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无 人 驾驶 车 辆 技术 在 智能 交通 系统 和 军事 领域 有 着 广 
阔 的 应 用 前 景 。 著 者 所 在 北京 理工 大 学 机 械 与 车 辆 学 院 知 
能 车 辆 研究 所 (Intelligent Vehicle Research Center, Beijing 
Institute of Technology) 自 1990 年 开始 进行 无 人 驾驶 车 辆 技 
术 研 究 和 探索 ， 并 且 于 2009—2013 年 连续 5 届 参 加 国家 自 
然 科 学 基金 委 主办 的 “中 国 智 能 车 未 来 挑战 赛 ”， 取 得 良 
好 成 绩 ， 其 中 在 2013 年 比赛 中 获得 冠军 。 

在 研究 过 程 中 ， 我 们 认识 到 无 人 驾驶 车 辆 的 环境 感 
知 、 规 划 决 策 方法 与 车 辆 平台 控制 是 密切 相关 的 ， 特 别 是 
高 速 无 人 车 辆 的 运动 学 与 动力 学 特性 不 仅 影响 到 感知 算 
法 ， 而 且 与 规划 算法 和 跟踪 控制 算法 有 着 内 在 的 联系 。 车 
辆 运动 学 约束 与 动力 学 约束 如 果 不 能 有 效 体现 在 运动 规划 
与 控制 算法 、 传 感 器 感知 算法 中 ， 就 无 法 得 到 很 好 的 控制 
效果 。 

模型 预测 控制 有 其 天 然 的 多 模型 约束 处 理 优势 ， 能 够 
与 规划 控制 、 感 知 过 程 的 传感器 数据 预 处 理 算法 很 好 地 结 
合 ， 是 在 无 人 驾驶 车 辆 控制 过 程 中 体现 车 辆 运动 学 与 动力 
学 约束 的 理想 方法 ; 但 模型 预测 控制 算法 较为 复杂 ， 计 算 
方法 复杂 ， 对 于 不 是 从 事 控制 理论 学 习 的 工科 学 者 比较 抽 
象 。 我 们 将 在 无 人 驾驶 车 辆 研究 过 程 中 应 用 模型 预测 控制 
理论 的 基础 概念 和 方法 、 算 法 代码 整理 出 来 ， 一 方面 给 课 
题 组 形成 较为 系统 的 学 习 资料 ， 方 便 后 续 研究 ; 另 一 方面 
也 可 以 供 从 事 移动 机 器 人 、 无 人 车 辆 (包括 无 人 机 、 无 人 
4t) 的 研究 者 们 参考 。 另 外 ， 也 可 作为 车 辆 工程 、 自 动 控 
制 等 专业 高 年 级 本 科 生 和 研究 生 学 习 模 型 预测 控制 的 辅助 
资料 。 

除 封面 署名 作者 外 ， 实 验 室 在 读 研 究 生 刘 凯 、 孙 银 
db, ERI, Hb. EXTR. IKSHEA 5 T3 23 0 9m 
稿 写 作 、 代 码 测 试 和 全 书 校 对 ， 于 宏 哺 提 供 附录 A 说 明 ; 
陈 慧 岩 教授 和 能 光明 副教授 对 全 书 内 容 进行 了 审 校 ; 清华 
大 学 汽车 研究 所 李 升 波 博 士 审阅 了 本 书 部 分 内 容 ， 给 出 了 
很 中 肯 的 修改 意见 。 在 此 一 并 致谢 。 

本 书 的 研究 工作 得 到 国家 自然 科学 基金 项 目 “高 速 地 
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面 车 辆 主动 危险 规避 最 优 运动 规划 与 控制 的 动力 学 模型 分 
M" (51275041) 资助 ， 是 该 项 目前 期 的 初步 研究 成 果 。 
后 续 工作 将 进一步 在 高 速 车 辆 控制 过 程 中 应 用 模型 预测 控 
制 方法 。 本 书 的 出 版 ， 也 得 到 国家 自然 科学 基金 项 目 “ 复 
杂交 通 条 件 下 弱 环 境 约 束 区 域 无 人 驾驶 车 辆 相对 定位 方法 
关键 问题 的 研究 ” (61304194) 部 分 资助 。 该 项 目 提 供 了 
实验 车 辆 的 定位 和 感知 方法 基础 ， 本 书 所 述 工作 同时 也 与 
Adr 3 gk db T 2011 一 2012 年 在 美国 麻 省 理工 学 院 Kar 
Iagnemma 博士 主持 的 Robotic Mobility Group 课题 组 访问 研 
究 期 间 从 事 的 研究 工作 相关 。 该 研究 工作 促成 了 基金 项 目 
“51275041” 的 申报 ， 同 时 这 些 研究 成 果 也 被 收录 在 本 
书 中 。 
代码 下 载 地 址 : http://www. bitpress. com. cns 
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无 人 驾驶 车 辆 (Self-driving Vehicle) 是 地 面 无 人 车 辆 的 一 种 ， 在 未 来 智能 
交通 系统 中 有 着 广阔 的 应 用 前 景 Ne 无 人 驾驶 技术 测 
试 样 车 ， 互 联网 与 移动 通信 公司 也 积极 涉足 这 一 领域 。 本 章 首 先 介绍 无 人 车 辆 
通用 概念 、 无 人 驾驶 车 pigs 辆 的 关系 ， 说 明 无 人 车 辆 的 主要 组 成 部 分 ， 
即 任务 决策 、 环 境 感知 、 路 径 规划 、 路 径 跟 中 与 车 辆 平台 控制 子 系统 ， 总 结 
无 人 驾驶 车 辆 的 特点 ， 最 后 介绍 模型 预测 控制 理论 在 无 人 驾驶 车 辆 控制 过 程 中 
的 应 用 情况 


1.1 无 人 驾驶 车 辆 


1.1.1 无 人 车 辆 通用 概念 

无 人 车 辆 (Unmanned Vehicle) 、 根 据 其 行 台 环境 的 不 同 ， 可 以 分 为 空中 
无 人 机 (Unmanned Aerial Vehicle) 、 水 面 无 人 艇 (Unmanned Surface Vehicle ) 、 
Ik FEAH k hr (Unmanned Underwater Vehicle) 和 地 而 无 人 车 辆 (Unmanned 
Ground. Vehicle) 。 无 人 车 辆 是 一 种 可 以 较 融 速度 或 高 速 移动 的 机 器 人 ， 能 够 感 
知行 驶 环境 ， 进 行 自 主 决 策 ， dinis fcd 4-4 sU] HH (S, US 
设 定 的 目的 地 ， 完 成 预定 任务 。 与 机 器 人 类 似 ， 无 人 车 辆 可 以 独立 或 者 协调 合 
作 完 成 预定 任务 ， 同 时 ， 震 要 指出 的 是 ， 副 操作 也 是 无 人 车 辆 的 一 种 重要 控制 
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方式 。 遥 操作 无 人 车 辆 不 等 同 于 完全 的 遥控 车 辆 ， 而 是 在 自主 行驶 无 人 车 辆 基 
础 上 ， 增 加 了 人 在 回路 的 任务 决策 、 环 境 感知 与 规划 、 控 制 等 功能 ， 是 一 种 提 
升 自主 无 人 车 辆 控制 能 力 的 人 机 交互 方式 , 或 者 被 称 为 人 在 回路 的 决策 、 规 划 
与 控制 方法 '" 。 

根据 以 上 定义 ， 这 里 将 无 人 车 辆 系统 分 为 任务 决策 、 环 境 感知 、 路 径 规 
划 ， 以 及 车 辆 控制 与 平台 4 个 子 系统 。 

(1) 任务 决策 子 系统 

无 人 车 辆 首先 是 一 个 任务 平台 ,在 军事 领域 ， 能 够 完成 侦察 、 打 击 、 排 暴 
和 后 勤 保障 等 任务 ， 空 中 、 水 面 、 水 下 、 地 面 的 各 种 无 人 车 辆 之 间 ， 或 者 无 人 
车 辆 与 有 人 车 辆 之 间 能 够 进行 任务 分 配 与 协调 ,不 同 功 能 ( 异 构 ) 或 相同 功 
能 ( 同 构 ) 的 车 辆 协同 完成 规定 任务 ; 在 民用 领域 , 无 人 车 辆 也 可 以 完成 农 
业 灌 溉 、 采 矿 、 码 头 运 输 、 交 通 运 输 等 任务 。 多 无 人 车 辆 协同 完成 任务 时 ,上 
有 更 高 的 效率 。 其 任务 分 配 与 协调 在 这 里 不 做 详细 介绍 ， 可 以 参阅 著者 前 期 研 
究 文献 [2, 3, 4]. 

当 任 务 分 配 到 单 台 无 人 车 辆 后 ， 无 人 车 辆 就 需要 根据 任务 特性 、 自 身 功能 
条 件 及 已 知 环境 信息 进行 任务 决策 与 规划 。 无 人 车 辆 在 任务 决策 与 规划 时 ， 任 
务 完 成 往往 与 车 辆 平台 全 局 路 径 规划 是 关联 的 。 比 如 军用 无 人 侦察 车 辆 任务 规 
划 包 括 两 部 分 内 容 : 一 是 根据 车 辆 平台 任务 载荷 特点 确定 完成 任务 的 方式 ; 二 
是 确定 完成 任务 的 全 局 路 径 ， 进 行 初始 全 局 路 径 规划 。 任 务 决策 与 规划 是 一 种 
动态 规划 ， 需 要 根据 任务 和 全 局 环境 信息 的 变化 进行 调整 。 以 完成 侦察 任务 为 
例 ， 无 人 车 辆 需要 遍历 侦察 区 域 ， 同 时 需要 规划 出 遍历 这 些 区 域 的 最 短 或 最 经 
济 全 局 路 径 。 

(2) 环境 感知 子 系统 

和 有 人 操控 车 辆 一 样 ， 无 人 车 辆 需要 实时 得 到 行驶 环境 信息 。 环 境 信息 一 
般 可 以 通过 以 下 两 种 途径 获得 : 一 是 通过 无 人 车 辆 环境 感知 子 系统 利用 车 载 传 
感 器 系统 获取 行驶 环境 信息 ， 结 合 环境 模型 对 传感器 信息 进行 融合 ， 理 解 和 识 
别 行驶 环境 ; 二 是 通过 通信 网 络 提供 的 外 部 环境 信息 ， 例 如 车 联网 给 无 人 驾 怠 
车 辆 提供 的 前 方 道路 拥堵 情况 和 周围 车 辆 行驶 趋势 ， 路 基 交 通 设 施 发 送 的 路 口 
交通 信号 灯 情 况 和 变化 趋势 ， 或 者 地 面 指 挥 系统 给 无 人 机 提供 的 大 范围 空域 环 
境 信息 。 

环境 感知 子 系统 利用 上 述 环境 信息 ， 并 结合 先 验 环 境 模型 ， 就 能 对 行 怠 环 
境 进 行 理解 识别 。 环 境 感 知 是 无 人 车 辆 可 靠 行 豫 的 关键 。 在 各 种 无 人 车 辆 中 ， 
空中 无 人 机 的 行驶 环境 识别 较为 简单 。 这 也 是 现在 无 人 机 无 论 是 在 军事 领域 还 
是 在 民用 领域 都 已 经 得 到 广泛 应 用 的 原因 。 地 面 无 人 车 辆 的 环境 感知 是 最 复杂 
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的 :以 行驶 在 城市 环境 的 无 人 驾驶 车 辆 为 例 ， 如 果 不 依赖 外 部 网 络 提供 的 环境 
信息 ， 独 立 的 无 人 驾驶 车 辆 需要 识别 行驶 道路 及 与 交通 规则 相关 的 各 类 环境 信 
A. 包括 车 道 线 以 及 无 车 道 线 情况 下 的 道路 识别 、 路 沿 检测 ""”、 交 通 标 
识 与 信号 灯 检 测 “” ， 以 及 行人 、 车 辆 检测 等， 而 且 要 综合 多 种 道路 交通 
要 素 对 比较 复杂 的 环境 进行 理解 ， 为 路 径 规划 子 系统 提供 行驶 区 域 信 息 和 障碍 
物 信 息 

(3) 路 径 规 划 子 系统 

路 径 规划 (Path Planning). 是 指 在 具有 障碍 物 的 环境 中 ,按照 一 定 的 评价 
标准 ， 比 如 路 径 长 度 最 短 或 能 量 消耗 最 少 原则 等 ， 寻 找 一 条 从 起 始 状态 到 目标 
状态 的 无 碰撞 路 径 。 无 人 车 辆 路 径 规 划 主 要 继承 了 机 器 人 研究 领域 关于 路 径 规 
划 的 研究 成 果 “” 、 — REA Og Ae der d (6 ML dap 0709099097 和 局 部 路 径 规 
X 17. 4g CR X X f i] HE pic (1 59 de 3 LaL] — 904). TE EC 
情况 下 ， 无 人 系统 全 局 路 径 规划 和 任务 决策 与 规划 是 相 联系 的 ， 二 者 结合 得 到 
无 人 车 辆 的 全 局 路 径 信 息 ; 而 局 部 路 径 规 划 则 是 在 无 人 车 辆 周围 局 部 环境 里 进 
行 的 

全 局 路 径 规划 是 在 地 图 已 知 的 情况 下 ， 利 用 已 知 局 部 信息 ， 如 障碍 物 位 置 
和 道路 边界 ， 确 定 可 行 和 最 优 的 路 径 ; 但 是 当 环 境 发 生变 化 ， 如 出 现 未 知 障碍 
物 时 ， 就 必须 通过 局 部 路 径 规 划 来 生成 无 人 车 辆 的 局 部 行驶 路 径 或 轨迹 。 局 部 
路 径 规划 是 在 全 局 路 径 引 导 下 ， 依 据 传感器 感知 到 的 局 部 环境 信息 来 实时 生成 
车 辆 所 需要 行驶 的 路 径 。 在 规划 过 程 中 不 仅 要 考虑 影响 当前 任务 完成 的 最 优 原 
则 ， 如 路 径 最 短 、 能 源 消耗 最 少 ， 或 者 车 辆 最 安全 等 ， 而 且 要 考虑 动态 环境 带 
来 的 约束 问题 。 

路 径 规划 技术 的 研究 重点 主要 包括 环境 建 模 和 路 径 搜索 策略 两 个 子 问 题 ， 
如 图 1. 1 所 示 。 路 径 规划 与 障碍 物 环境 信息 密切 相关 ， 因 此 障 但 物 环 境 的 建 模 
方法 是 路 径 规划 问题 研究 中 的 一 个 关键 问题 。 目 前 ， 最 常用 的 3 种 环境 建 模 方 
法 是 单元 分 解法 (Cell Decomposition) 、 道 路 图 法 (Roadmap) 和 人 工 势 场 法 
( Potential Fields) ” 。 其 中 人 工 势 场 法 在 实时 避 障 方面 具有 独特 优势 。 在 此 基 
GE. xig pFuESLJI (Virtual Force Fields, VFF), ^c ft dt 7; [0 i: 
(Vector Field Histogram, VFH) 和 向 量 极 坐标 直方 图 法 (Vector Polar Histogram, 
VPH) "7 这 些 方法 在 机 器 人 和 无 人 藻 辆 障碍 规避 方面 得 到 大 量 应 用 。 

在 无 人 车 辆 和 机 器 人 局 部 路 径 规 划 过 程 中 ， 还 需要 考虑 的 一 个 重要 问题 是 
运动 规划 (Motion Planning) ， 即 局 部 路 径 规划 要 满足 无 人 车 辆 的 运动 学 与 动 
力学 约束 条 件 。 文献 [16] 提出 的 满足 无 人 车辆 运动 微分 约束 的 纵横 向 协同 
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图 1.1 路 径 规划 、 环 境 信 息 与 环境 建 模 的 关系 


规划 算法 就 是 一 种 典型 的 局 部 路 径 规划 的 运动 规划 方法 。 将 模型 预测 控制 理论 
应 用 在 无 人 车 辆 运动 规划 时 ， 也 将 重点 探索 该 方法 在 处 理 运 动 学 和 动力 学 约束 
的 途径 ， 在 后 续 章 节 中 将 对 其 重点 阐述 。 

(4) 车 辆 平台 控制 子 系统 

车 辆 控制 主要 是 控制 车 辆 平台 跟踪 路 径 规划 子 系统 得 到 的 路 径 ， 也 即 车 
辆 的 横向 与 纵向 控制 。 和 机 器 人 控制 一 样 ， 无 人 车 辆 也 存在 着 路 径 
(Path) 与 轨迹 (Trajectory) 的 区 分 。 本 书 约定 轨迹 是 路 径 的 一 种 ， 即 同时 
考虑 空间 和 时 间 因 素 的 路 径 。 路 径 跟 踪 实 质 是 通过 控制 车 辆 的 运动 来 减少 车 
辆 与 参考 路 径 之 间 的 空间 上 的 误差 。 如 果 考 虑 轨迹 ， 则 还 包括 时 间 误 差 。 因 
模型 预测 控制 重点 应 用 在 跟踪 控制 方法 中 ， 后 续 章 节 将 继续 对 这 一 点 进行 详 
细 说 明 。 

车 辆 平台 是 无 人 车 辆 的 重要 组 成 部 分 。 环 境 感知 、 决 策 规划 及 控制 必须 
与 车 辆 平台 进行 一 体 化 设计 。 各 种 无 人 车 辆 在 行驶 环境 中 ， 以 较 高 速度 行驶 
时 ， 都 会 与 环境 发 生 相 互 作用 ， 这 时 车 辆 动力 学 与 运动 学 特性 就 会 影响 到 环 
境 感知 、 决 策 规划 和 控制 效果 。 以 最 为 复杂 的 地 面 无 人 车 辆 为 例 ， 高 速 行驶 
的 车 辆 执行 机 构 的 控制 输入 、 轮 胎 与 地 面 摩擦 引起 的 滑 移 、 横 向 加 速度 引起 
的 侧 倾 等 动力 学 非 线 性 约束 条 件 比 低速 时 更 加 苛刻 。 因 此 ， 无 人 车 辆 一 方面 
要 在 运动 规划 阶段 计算 出 满足 车 辆 动力 学 和 运动 学 约束 的 无 碰撞 运动 轨迹 ， 
另 一 方面 需要 在 跟踪 阶段 生成 满足 非 线 性 动力 学 约束 和 执行 机 构 极 限 约束 的 
控制 量 。 


1.1.2 考虑 乘坐 舒适 性 的 无 人 驾驶 车 辆 

由 上 面 简要 介绍 的 通用 无 人 车 辆 概念 及 其 主要 组 成 部 分 可 以 看 出 ， 无 人 车 
辆 种 类 有 很 多 。 有 的 车 辆 有 乘坐 人 员 ， 但 不 直接 驾驶 ; 而 有 的 没有 乘坐 人 员 ， 
完全 不 需要 考虑 人 员 的 乘坐 感受 。 根 据 其 特点 不 同 ， 规 划 与 控制 时 需 考虑 的 要 
素 也 不 同 。 如 军用 无 人 车 辆 在 危险 环境 中 行驶 时 ， 只 需要 搭载 执行 任务 的 执行 
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机 构 、 传 感 器 以 及 车 辆 平台 本 身 能 够 适应 环境 ， 因 此 规划 与 控制 时 就 只 需要 考 
虑 机 硕 的 承受 能 力 ， 而 不 宕 要 考虑 人 的 乘坐 舒适 性 ， 这 样 有 可 能 形成 更 高 的 
机 动能 力 。 本 书 主要 以 与 普通 乘 用 车 类 似 的 地 面 无 人 车 辆 为 对 象 进行 建 模 ， 
并 且 约 定 该 车 辆 为 前 轮转 向 ， 即 一 种 能 够 自动 或 自主 驾驶 的 无 人 驾驶 车 辆 
( Self-driving Vehicle ) 。 虽 然 没 有 驾驶 员 ， 但 还 有 乘客 ， 因 此 需要 考虑 人 的 乘坐 
舒适 性 

选择 有 人 乘坐 的 无 人 驾驶 车 辆 (Self-driving Vehicle) 还 有 两 个 原因 : 一 是 
这 类 车 辆 的 运动 学 模型 和 动力 学 模型 都 比较 成 熟 ， 在 仿真 软件 CarSim 和 Adams 
中 都 有 相应 的 模型 ， 便 于 我 们 重点 介绍 模型 预测 控制 理论 在 无 人 驾驶 车 辆 控 
制 中 的 应 用 ; 二 是 著者 前 期 公开 发 表 的 工作 成 果 也 都 是 基于 这 类 车 辆 平台 ， 
并 且 有 多 辆 可 以 实际 测试 的 无 人 驾驶 车 辆 可 作为 验证 平台 。 图 1.2 所 示 为 北 
京 理 工大 学 智能 车 辆 研究 所 与 比亚迪 汽车 有 限 公 司 共 同 研发 的 Ray 5726 A 55 
Jh AE fA 








图 1.2 无 人 驾驶 车 辆 Ray (4K 2013 年 “中 国 智能 车 未 来 挑战 赛 ” 冠 车 ) 


1.2 路 径 跟 踪 与 轨迹 跟 蹊 


模型 预测 控制 理论 将 主要 应 用 在 无 人 驾驶 车 辆 的 路 径 或 轨迹 生成 与 跟踪 控 
制 中 。 这 里 首先 介绍 无 人 车 辆 路 径 与 轨迹 的 概念 ， 然 后 说 明 路 径 跟 踪 (Path 
Following) 与 轨迹 跟踪 (Trajectory Tracking). 的 特点 

1.2.1 路 径 规 划 与 轨迹 规划 


路 径 与 轨迹 延续 了 机 器 人 控制 的 概念 。 对 于 无 人 车 辆 来 说 ， 全 局 路 径 点 只 
机 包含 空间 位 置信 息 即 可 ， 也 可 以 包含 姿态 信息 ， 而 不 需要 与 时 间 相关 ， 但 局 
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部 规划 时 ， 则 可 以 考虑 时 间 信 息 。 这 里 规定 轨迹 点 也 是 一 种 路 径 点 ， 即 当 路 径 
点 信息 中 加 入 了 时 间 约 束 ， 就 可 以 被 称 为 轨迹 点 。 从 这 个 角度 理解 ， 轨 迹 规划 
就 是 一 种 路 径 规划 ， 当 路 径 规划 过 程 要 满足 无 人 车 辆 的 纵向 和 横向 动力 学 约束 
时 ， 就 成 为 轨迹 规划 。 路 径 规划 与 轨迹 规划 既 可 以 在 状态 空间 中 表示 ， 也 
可 以 在 笛 卡 尔 坐标 系 中 表示 " 。 

无 人 车 辆 纵向 速度 规划 与 时 间 密 切 相关 ， 而 车 辆 横向 动力 学 对 纵向 速度 的 
影响 非常 大 。 文 献 [16] 提出 一 种 基于 运动 微分 约束 的 无 人 车 辆 纵 、 横 向 协 
同 规划 算法 ,实际 上 考虑 了 无 人 车 辆 通过 某 一 路 径 点 的 速度 要 求 ， 是 一 种 轨迹 
规划 算法 。 


1.2.2 路 径 跟踪 与 轨迹 跟踪 


路 径 跟踪 过 程 中 ， 参 考 路 径 (Reference Path) 曲线 可 与 时 间 参 数 无 关 ， 
跟踪 控制 时 ， 可 以 假设 无 人 车 辆 以 当前 速度 匀速 前 进 ， 以 一 定 的 代价 规则 形成 
行驶 路 径 趋 近 于 参考 路 径 ; 而 轨迹 跟踪 时 ， 参 考 路 径 曲 线 与 时 间 和 空间 均 相 
关 ， 并 要 求 无 人 车 辆 在 规定 的 时 间 内 到 达 某 一 预先 设 定好 的 参考 路 径 点 。 

路 径 跟踪 不 同 于 轨迹 跟踪 ， 不 受制 于 时 间 约 束 ， 只 需要 在 一 定 误差 范围 内 
跟踪 期 望 路 径 。 路 径 跟 踪 中 的 运动 控制 就 是 寻找 一 个 有 界 的 控制 输入 序列 ， 以 
使 无 人 车 辆 从 一 个 初始 位 形 到 设 定 的 期 望 位 形 。 文 献 [26] 指出 ,无 人 车 辆 
系统 是 一 个 从 驱动 的 非 完整 约束 系统 ， 同 时 也 是 一 个 零 动 力学 系统 。 轨 迹 跟踪 
问题 会 受 限 于 这 种 不 稳定 的 零 动 力学 约束 。 


1.3 模型 预测 控制 在 无 人 驾驶 车 辆 
运动 规划 与 控制 中 的 应 用 


车 辆 动力 学 模型 分 析 对 于 解决 无 人 驾驶 车 辆 运动 规划 与 控制 问题 具有 十 分 
关键 的 作用 。 首 先 ， 在 规划 与 控制 系统 中 引入 动力 学 模型 可 以 通过 模型 的 等 效 
约束 转化 减少 规划 与 控制 的 计算 量 ， 提 高 系统 的 实时 性 。 其 次 ， 以 准确 的 动力 
学 模型 作为 预测 模型 ， 可 以 提高 控制 器 对 车 辆 未 来 行为 的 预测 能 力 ， 进 而 在 保 
证 车 辆 稳定 运行 的 同时 ， 充 分 发 挥 车 辆 的 机 动 潜能 ， 比 如 高 速 运行 状况 下 的 主 
动 危险 规避 运动 规划 与 控制 ， 充 分 体现 车 辆 平台 运动 学 与 动力 学 约束 条 件 的 
影响 。 

模型 预测 控制 最 明显 的 优点 是 能 在 控制 过 程 中 增加 多 种 约束 。 无 人 车 辆 在 
低速 时 ， 车 辆 平台 运动 学 约束 影响 较 大 ， 而 随 着 速度 的 增加 ， 动 力学 特性 对 运 
动 规划 与 控制 的 影响 就 越 明显 ， 带 来 多 种 模式 的 约束 。 这 正 是 模型 预测 控制 在 
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无 人 车 辆 运动 规划 控制 方面 的 应 用 优势 。 下 面 介绍 地 面 无 人 驾驶 车 辆 的 运动 规 
划 和 控制 算法 研究 现状 ， 同 时 说 明 模 型 预测 控制 理论 在 这 些 算 法 中 的 应 用 
情況 


1.3.1 运动 规划 算法 的 模型 约束 


研究 无 人 驾驶 车 辆 的 运动 规划 问题 时 ， 利 用 感知 系统 提供 的 障碍 物 位 置 与 
运动 属性 、 路 面 特性 以 及 车 辆 本 身 状态 ， 可 计算 出 从 当前 位 置 到 局 部 目标 位 置 
的 没有 碰撞 的 行驶 轨迹 。 根 据 前 面 对 路 径 规划 与 轨迹 规划 的 特点 介绍 ， 一 旦 轨 
迹 被 确定 了 ， 车 辆 沿 着 路 径 的 运动 形式 也 就 被 确定 了 。 轨 迹 规划 既 可 以 是 在 路 
径 规划 后 的 二 次 规划 ， 也 可 以 根据 感知 信息 一 次 规划 获得 。 

(1) 路 径 规划 算法 约束 条 件 

无 人 驾驶 车 辆 的 局 部 路 径 规划 算法 不 同 于 一 般 移动 机 器 人 的 路 径 规划 问 
题 。 为 满足 车 辆 转向 过 程 中 的 平稳 性 以 及 安全 性 需求 ， 车 辆 运动 微分 约束 和 动 
力学 特性 约束 是 需要 考虑 的 重要 特征 ， 同 时 还 需要 考虑 车 辆 操纵 稳定 性 及 舒适 
性 问题 带 来 的 控制 约束 。 文献 [26] 利用 弹性 带 理论 (Elastic Band Theory) 
对 无 人 驾驶 车 辆 的 紧急 避 障 路 径 进行 规划 ， 在 规划 时 提出 将 路 径 的 局 部 曲率 变 
化 最 小 作为 约束 条 件 ， 从 而 提高 车 辆 紧急 避 障 时 的 操纵 稳定 性 。 文 献 [27] 
讨论 了 非 结 构 化 环境 下 基于 滚动 窗口 的 无 人 车 辆 路 径 规划 问题 ， 并 在 规划 过 程 
中 考虑 了 车 辆 的 安全 约束 

局 部 路 径 规划 算法 中 考虑 约束 的 y 
-种 思路 是 对 车 辆 行驶 曲线 进行 描 
述 ， 通 过 设计 评价 函数 选取 不 同 工 况 
下 的 行 台 曲线 完 成 规划 。 文献 [16] 
利用 5 次 多 项 式 描述 行驶 曲线 ， 将 问 
题 变 为 求 取 连接 两 个 确定 的 目标 点 之 


全 局 或 局 部 环 
aom 





间 的 曲线 ， 并 且 满 足 目 标点 处 有 固定 
的 航向 角 和 固定 曲率 的 要 求 ， 如 图 nte 
1.3 所 示 : 同时 ， 在 全 局 或 者 局 部 环 车 辆 当 


前 位 置 





境 中 的 参考 路 径 上 用 不 同 预 瞄 距离 确 
定 若干 个 预 瞄 位 姿 ， 对 于 每 个 预 瞄 位 
姿 沿 其 法 线 方 向 按照 不 同 横向 位 置 偏 
差 再 确定 若干 个 目标 位 姿 ， 各 目标 位 姿 航 向 与 预 瞄 位 次 相同 ， 计 算 当前 车 辆 位 
次 和 各 目标 位 姿 之 间 的 行驶 曲线 作为 候选 曲线 集合 ， 行 驶 曲线 需要 满足 车 辆 运 


图 1.3 基于 5 次 多 项 式 的 路 径 规 划 
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动 微分 约束 。 候 选 曲 线 集合 包含 了 用 不 同 预 眶 距离 生成 的 行驶 曲线 ， 而 对 于 同 
一 预 瞄 距 离 处 的 多 条 行驶 曲线 ， 其 目标 位 姿 相对 于 期 望 路 径 是 对 称 分 布 的 。 首 
先 对 各 条 行驶 曲线 进行 碰撞 分 析 ， 在 行驶 曲线 上 的 每 一 位 姿 处 增加 车 辆 的 宽度 
和 长 度 信 息 ， 与 构 形 空间 中 的 障碍 物 检测 结果 进行 对 比 ， 判 断 行驶 曲线 与 障碍 
物 发 生 碰撞 的 位 置 ， 仅 保留 此 位 置 以 前 的 行驶 曲线 。 然 后 ， 在 具有 相同 横向 位 
置 偏差 的 行驶 曲线 中 保留 最 长 的 一 条 用 于 路 径 评价 。 经 过 碰撞 安全 性 分 析 ， 在 每 
个 横向 位 置 偏差 仅 保留 了 一 条 满足 横向 安全 和 路 径 跟踪 要 求 的 行驶 曲线 。 车 辆 的 
实际 驾驶 行为 ， 如 跟随 或 超车 ， 则 由 选择 哪 一 条 行驶 曲线 作为 待 执行 路 径 实现 。 
德国 卡尔 斯 鲁 厄 大 学 在 DARPA 
Urban Challenge 的 参赛 车 辆 Annie- 
WAY 上 采用 了 基于 触须 原理 的 局 部 路 
径 规划 算法 “ ， 利 用 多 组 不 同 半径 的 
圆 弧 描述 不 同 车 速 下 的 行驶 路 径 ， 如 
图 1.4 所 示 。 文 献 [18] 对 该 算法 进 
行 了 改进 ， 使 之 适用 于 全 局 环境 未 知 
的 智能 车 自主 行驶 ,但 由 于 它 在 进行 
图 1.4 基于 触须 的 路 径 规划 触须 生成 时 采用 圆 弧 曲线 ， 而 忽略 了 
车 辆 转弯 半径 的 变化 ， 故 在 车 速 较 快 
时 容易 发 生 跟 踪 失 败 的 情况 。 这 也 说 明 在 高 速 时 还 需要 进一步 考虑 车 辆 动力 学 
特性 的 约束 。 
文献 [29] 结合 预测 控制 理论 进行 无 人 车 辆 的 局 部 路 径 规划 。 借 鉴 预 测 
控制 的 滚动 优化 原理 ， 提 出 了 基于 滚动 窗口 的 移动 机 器 人 路 径 规划 方法 。 该 方 
法 在 全 局 环境 未 知 时 能 够 充分 利用 已 探知 的 局 部 信息 ， 有 效 地 结合 优化 与 反馈 
机 制 ， 不 仅 使 局 部 规划 的 计算 量 保持 在 较 低 水 平 ， 而 且 保证 了 全 局 的 收敛 性 ， 
获得 了 较 好 的 规划 效果 。 文 献 [30] 同样 借鉴 了 滚动 优化 的 原理 ， 提 出 了 一 
种 带 约 束 曲 线 拟 合 的 路 径 规划 方式 ， 如 图 1. 5 所 示 。 该 方法 通过 在 规划 中 引入 
几何 约束 ， 如 起 始 位 姿 约束 、 最 小 转弯 半径 约束 等 ， 有 效 地 避免 了 在 跟踪 过 程 
中 同时 考虑 位 置 和 航向 误差 的 问题 。 
(2) 轨迹 规划 算法 约束 条 件 
轨迹 规划 问题 一 般 是 在 考虑 系统 某 项 性 能 最 优 的 同时 ， 加 入 边界 约束 、 环 
境 几何 约束 以 及 系统 的 动力 学 约束 等 非 线性 约束 ， 最 终 构成 一 个 非 线性 最 优化 
问题 。 因 此 ， 轨 迹 规划 的 主要 研究 方向 集中 在 非 线 性 最 优 求解 和 约束 条 件 等 价 
转换 上 。 轨 迹 规划 问题 一 般 无 法 找到 显 式 解 ， 普 遍 的 做 法 就 是 将 这 一 优化 问题 
转换 为 非 线 性 规划 问题 ， 进 而 通过 相应 的 非 线性 规划 求解 器 求解 。 





可 行驶 触须 


5rjs.cn 000000 


第 I 章 无 人 驾驶 车 辆 与 模型 预测 控制 





© 


Y 探测 区 域 





图 1.5 zz e gp te M 


文献 [31] 采用 直接 法 ， 利 用 多 项 式 直接 对 系统 状态 进行 参数 化 ， 进 而 
将 轨迹 规划 问题 转换 为 数学 规划 问题 。 文献 [32] 在 借鉴 样 条 法 原理 的 基础 
上 ， 通 过 局 部 近似 的 方法 构建 全 局 连续 的 优化 轨迹 。 虽 然 这 种 转换 解决 了 在 轨 
迹 空间 优化 的 难题 ,但 是 其 优化 的 速度 依赖 于 参数 空间 的 维度 。 为 此 ，Fahroo 
等 人 在 对 非 线 性 系统 平坦 性 人 研究 的 基础 上 ， 给 出 了 平坦 系统 的 轨迹 规划 方 
bk. 其 主要 优点 就 是 通过 系统 的 平坦 性 将 高 维度 空间 的 参数 优化 问题 降 为 
低 维度 参数 空间 的 优化 问题 ， 从 而 提高 了 优化 速度 。 吉 林 大 学 陈 虹 等 也 应 用 微 
分 平坦 理论 对 不 同形 式 的 机 器 人 进行 了 轨迹 规划 方面 的 研究 “™ 

侧 倾 约 束 下 的 运动 规划 也 是 高 速 运动 车 辆 需要 考虑 的 一 个 重要 约束 。 侧 倾 
主要 采用 横向 载荷 转移 率 (LTR ) 、 侧 倾 朋 和 横向 加 速度 作为 判断 标准 。Tahi- 
rovic 在 被 动 侧 翻 控 制 基础 上 进行 了 模型 预测 改进 运动 规划 ;Iagnemma ** 
在 机 器 人 车 辆 运动 轨迹 规划 时 ， 设 计 车 辆 动力 学 参数 值 窗口 ， 采 用 速度 - 曲率 
轨迹 方法 计算 运动 轨迹 。 由 于 采用 窗口 值 只 考虑 曲率 ， 所 以 其 计算 速度 快 ， 但 
并 不 具备 车 辆 控制 稳定 的 最 优 性 

在 约束 条 件 处 理 方面 ， 基 于 简化 运动 学 模型 满足 车 辆 非 完整 约束 的 方法 比 
较 普 遍 “” .假设 车 辆 在 运动 过 程 中 没有 滑 移 ， 那 么 就 是 非 完 整 ( Nonholo- 
nomic) 动力 学 约束 ， 但 是 这 种 方法 不 适用 于 高 速 行驶 车 辆 ， 因 为 此 时 车 辆 的 
约束 已 经 不 再 局 限于 非 完整 约 束 ， 车 辆 的 横向 动力 学 成 为 规划 中 必须 考虑 的 因 
X. 文献 [39] 和 [40] 在 考虑 车 辆 动力 学 约束 情况 下 对 车 辆 的 行驶 轨迹 进 
行 了 规划 ， 实 现 了 连续 曲率 的 轨迹 规划 ， 

为 了 减少 动力 学 约束 的 复杂 程度 ， 忽 略 车 辆 尺寸 信息 简化 而 来 的 点 质量 模 
型 也 在 轨迹 规划 中 得 到 了 应 用 ”其 受 力 分 析 如 图 1.6 所 示 。 从 图 1.6 中 
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可 以 看 出 ,该 模型 考虑 了 车 辆 的 侧 向 受 力 ， 并 且 将 约束 条 件 转换 为 加 速度 贺 
( Acceleration Circle Constraint ) 。 应 用 该 模型 能 够 顺利 完成 避 障 任务 。 点 质量 模 
型 相 比 于 普通 自行 车 模型 计算 量 更 小 ， 可 以 有 效 地 减少 车 辆 非 线 性 动力 学 模型 
的 复杂 性 。 将 车 辆 动力 学 模型 近似 为 点 质量 模型 ， 同 时 轮胎 摩擦 圆 约束 也 被 映 
射 到 加 速度 约束 上 ， 以 及 其 他 的 一 些 技巧 均 可 用 于 简化 计算 。 





图 1.6 点 质量 模型 受 力 分 析 


(3) 运动 规划 算法 及 存在 的 问题 小 结 

从 上 面 可 以 看 出 ， 轨 迹 规划 方面 的 研究 一 般 都 按照 局 部 路 径 规划 方法 进 
行 ， 然 后 在 考虑 多 种 约束 条 件 的 情况 下 对 其 进行 轨迹 化 。 与 低速 行驶 的 移动 机 
器 人 不 同 的 是 ,无 人 驾驶 车 辆 的 路 径 和 轨迹 规划 更 多 采用 多 项 式 行驶 曲线 的 方 
式 进 行 拟 合 ， 同 时 考虑 了 执行 机 构 的 约束 。 这 种 规划 的 特点 是 能 够 保证 车 辆 平 
顺 行 驶 ， 不 会 出 现 突然 转向 。 为 了 进一步 提高 系统 的 实时 性 ， 微 分 平坦 、 点 质 
量 模型 等 相关 理论 也 被 应 用 在 了 轨迹 规划 的 过 程 中 。 

普通 规划 方法 在 高 速 无 人 车 辆 运动 规划 中 普遍 存在 的 问题 是 实时 性 达 不 到 
要 求 ， 特 别 是 紧急 情形 下 的 危险 规避 运动 规划 对 实时 性 要 求 更 高 。 基 于 动力 学 
优化 模型 的 规划 方法 均 考 虑 了 轮胎 摩擦 圆 约束 ， 采 用 受 加 速度 约束 的 点 质量 模 
型 来 近似 轮胎 摩擦 圆 约 束 ， 但 没有 对 高 速 车 辆 动力 学 模型 结合 运动 规划 做 深入 
的 最 优 分 析 。 侧 倾 约束 下 的 运动 规划 方法 研究 较 少 ， 且 主要 集中 在 越野 环境 ， 
仅仅 是 根据 阀 值 窗 口 范围 进行 运动 规划 ， 也 同样 没有 根据 车 辆 动力 学 理论 的 稳 
定性 最 优 原则 产生 运动 轨迹 。 此 外 ， 同 时 考虑 滑 移 与 侧 倾 的 研究 则 更 少 

点 质量 模型 最 优 控制 特性 分 析 也 是 运动 规划 中 的 一 个 重要 因素 。 用 加 速度 
值 有 界 且 方 向 可 控 的 点 质量 模型 进行 最 优 控制 特性 分 析 和 证 明 ， 能 保证 建立 在 
此 基础 上 的 相关 算法 的 可 靠 应 用 。Bryson 和 Ho 给 出 了 几 种 情形 的 证 明 并 且 将 
其 应 用 到 运载 火箭 的 发 射 中 ， 用 于 推力 控制 分 析 只 ， 可 以 供 地 面 车 辆 的 运动 
规划 最 优 特性 分 析 参 考 。 
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1.3.2 ”轨迹 跟踪 控制 的 模型 约束 


轨迹 跟踪 控制 的 目的 是 让 无 人 驾驶 车 辆 跟踪 由 规划 算法 得 到 的 轨迹 。 其 主 
要 任务 是 根据 车 辆 的 运动 学 和 动力 学 约束 输出 相应 的 控制 参数 ， 如 前 轮 偏 角 、 
车 轮 制 动 力 等 。 目 前 应 用 较 多 的 控制 算法 有 PID 控制 算法 、 滑 模 控 制 算 法 、 神 
经 网 络 控制 算法 等 。 这 些 方 法 对 参数 和 环境 的 依赖 程度 较 高 ， 当 环境 发 生 较 大 
变化 时 不 能 很 好 地 适应 新 状态 下 的 轨迹 跟踪 ; 同时 ， 地 面 无 人 车 辆 在 运动 过 程 
中 受到 运动 学 约束 以 及 执行 机 构 的 约束 ， 在 高 速 运动 情况 下 还 需要 考虑 相应 的 
动力 学 约束 -上 述 方 法 对 这 些 约束 难以 处 理 。 由 于 基于 模型 预测 控制 的 轨迹 跟 
DARE 迹 的 预测 和 处 理 多 目标 约束 条 件 的 能 力 较 强 ， 故 逐渐 成 为 研究 

在 应 用 车 辆 模型 进行 预测 时 ， 是 否 考 虑 轮胎 滑 移 是 一 个 非常 重要 的 约束 条 
件 . 这 也 是 实际 车 辆 测试 的 一 个 难点 问题 。 轮 胎 滑 移 率 也 是 驱动 防滑 系统 
(ASR) 及 防 抱 死 制 动 系统 (ABS) 的 关键 参数 。 考 虑 到 轮胎 柔 度 和 摩擦 力 饱 
和 值 ， 轮 胎 纵 横向 耦合 摩擦 力 (驱动 力 ) 模型 一 般 为 滑 移 率 的 非 线性 函数 。 
T 日 较 广 的 是 Pacejka 提出 的 魔术 公式 轮胎 模型 。 

) ER 完整 动力 学 约束 轨迹 跟踪 

宛 整 (Nonholonomic ) 动力 学 约束 就 是 假设 车 辆 在 运动 过 程 中 没有 滑 
E. 很 多 在 平坦 路 而 运动 的 无 人 和 车辆 或 移动 机 絮 人 都 用 这 种 假设 设计 轨迹 跟踪 
控制 器 ， 特 别 是 纯 跟 踪 算 法 (Pure Pursuit) ， 已 经 被 成 功 地 应 用 在 很 多 机 器 人 
轨迹 跟踪 控制 中 ， 包 括 获 得 2005 年 DAPAR 沙漠 挑战 赛 冠军 的 斯 坦 福 大 学 的 
Stanley 赛车 - 

(2) 厘 擦 力 与 滑 移 率 非 线 性 约束 轨迹 跟踪 

带 有 纵 柳 向 耦合 滑 移 的 非 线性 轮胎 模型 在 轨迹 跟踪 控制 句 中 一 般 利用 数值 
优化 计算 方法 进行 解 算 . 文献 [46] 应 用 凸 二 次 锥 规划 优化 数值 计算 方法 ， 
而 且 在 控制 中 使 用 了 非 线 性 相合 模型 的 简化 模型 。 文 献 [47] 用 预测 控制 方 
法 为 由 主 车 辆 设计 适应 轮胎 摩擦 极 限 的 轨迹 跟踪 控制 器 ， 应 用 了 耦合 非 线性 轮 
胎 模 而 该 懂 击 较 复 杂 ，、Velenis 和 Frazzoli 通过 推导 出 无 人 驾驶 车 辆 在 转弯 时 
的 速度 ENIM ffo. pH f E EA. (EDI wipes 中 应 用 滑 模 控制 方法 能 
很 好 地 控制 车 辆 在 淖 移 状态 下 行驶 ”。， 基 主要 不 足 是 只 能 稳定 地 控制 均匀 滨 
移 的 运动 并 且 还 无 法 跟踪 时 弯曲 率 轨迹 。 

(3) 车 策动 力学 异型 简化 对 便 型 预测 控制 的 影响 

盾 测 模型 的 选择 也 是 基于 模型 预测 控制 的 轨迹 跟 踊 算法 需要 状 虚 的 关键 问 
题 之 一 ”使 用 复杂 的 模型 能 够 更 好 地 对 耕 辆 输出 进行 顶 测 ， 但 同时 也 增 大 了 控 
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制 器 的 计算 量 ， 实 时 性 难以 保证 ; 而 使 用 简单 的 模型 有 可 能 导致 跟踪 失败 。 针 
对 这 个 问题 ， 文 献 [49] 分 别 基 于 2 自由 度 自 行车 模型 和 14 自由 度 动力 学 模 
型 设计 了 轨迹 跟踪 控制 器 ， 然 后 对 轨迹 跟踪 效果 进行 对 比 ， 结 果 如 图 1.7 所 
示 。 其 中 , 图 1.7 (a) 为 没有 考虑 安全 约束 ， 而 图 1.7 (b) 中 两 者 都 加 入 安 
全 约束 。 可 以 看 出 ， 基 于 14 自由 度 模 型 的 控制 器 能 够 具备 更 好 的 避 障 效果 ; 
但 加 入 了 14 自由 度 模型 中 的 安全 约束 之 后 ，2 自由 度 模型 也 能 具备 相近 的 效 
果 并 且 计 算 时 间 优 势 明 显 。 因 此 ， 复 杂 的 预测 模型 并 不 是 最 好 的 选择 ， 对 车 辆 
动力 学 模型 进行 合理 简化 并 且 选 择 满 足 行驶 工 况 的 约束 条 件 才 是 研究 的 重点 。 
也 就 是 说 ， 在 模型 预测 控制 中 ， 模 型 约束 条 件 能 够 表达 控制 对 象 的 真实 物理 限 
制 即 可 。 





500 500 
400 400 
300 300 
E E 
一 一 
a ~ 
200 200 
100 100 
8 0 100 200 300 400 500 9 0 100 200 300 400 500 
x/m x/m 
(a) (b) 


图 1.7 2 自由 度 模型 与 14 自由 度 模 型 跟踪 结果 对 比 
(a) 未 加 入 安全 约束 ; (b) 加 入 安全 约束 


上 面 关 于 模型 复杂 度 对 控制 效果 的 分 析 为 控制 过 程 中 的 模型 简化 提供 了 依 
据 。 文献 [30] 通过 对 轮 式 移动 机 器 人 的 运动 学 模型 在 目标 轨迹 点 附近 进行 
线性 化 ， 得 到 了 离散 的 系统 状态 空间 模型 ; 采用 线性 模型 预测 控制 算法 进行 轨 
迹 跟踪 ， 并 通过 仿真 和 实 车 实验 分 别 验证 了 算法 的 可 靠 性 。 

在 动力 学 建 模 方面 ， 麻 省 理工 学 院 (Massachusetts Institute of Technology, 
MIT) 、 斯 坦 福 大 学 (Stanford University) 以 及 韩国 釜山 国立 大 学 等 做 了 深入 的 
研究 。MIT 的 Robotic Mobility Group 课题 组 通过 微分 平坦 理论 、 点 质量 模型 对 
车 辆 动力 学 模型 进行 合理 简化 ， 应 用 轮胎 的 摩擦 圆 约束 〈 如 图 1.8 所 示 ) 、 提 
高 了 系统 的 实时 性 ， 实 现在 高 速 情况 下 的 地 面 无 人 车 辆 危险 规避 ”轮胎 
摩擦 圆 代表 轮胎 与 地 面 摩擦 所 能 产生 的 最 大 牵引 力 。 由 于 牵引 力 可 以 为 任意 
方向 ， 以 轮胎 与 地 面 接触 中 心 点 为 圆心 ， 最 大 牵引 力 为 半径 得 到 的 圆 ， 就 是 
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轮胎 摩擦 圆 。 摩擦 圆 约束 是 指 轮胎 与 地 面 摩 た mg 
擦 时 纵向 与 横向 产生 的 耦合 力 必须 位 于 摩擦 C tiii die d 
圆 内 。 车 辆 转弯 、 加 速 、 减 速 时 ， 如 果 夺 全 ra 
力 超出 了 摩擦 圆 ， 车 辆 就 会 产生 滑 移 。 轮 胎 / 
摩擦 圆 会 因为 路 面 的 状况 、 轮 胎 的 接地 负荷 
或 者 轮胎 的 性 能 而 改变 。 例 如 在 冰雪 或 湿 滑 X 
的 路 面 ， 轮 胎 摩擦 圆 比 在 常规 路 面 行驶 
时 小 





在 如 图 1.9 所 示 的 高 速 车 辆 换 道 避 障 运 。“ Pu LEUR AR s IN 
— ; gis cadis : F 轮胎 纵向 摩擦 力 ”到 轮胎 法 向 作用 力 
动 轨迹 规划 中 ，D, 是 车 辆 与 前 方 障 碍 的 纵向 
IER, D, 是 本 车 道 与 旁 车 道 的 距离 ， 那 么 危 图 1.8 轮胎 摩擦 圆 约 束 
险 规 避 运 动 规划 问题 可 表述 为 : 首先 计算 在 满足 各 轮胎 摩擦 圆 约束 和 障碍 约束 的 
条 件 下 ， 车 辆 能够 安全 换 到 距离 为 D 的 车 
道 的 最 小 纵向 距离 D,,,。 当 调整 到 D, > 
D;i, 时 ,计算 车 辆 换 道 运动 轨迹 。 文 献 
153」 利用 二 次 锥 规划 方法 来 确定 作用 在 车 
辆 上 的 最 优 力 和 力矩 。 车 辆 用 刚体 表示 ， 
并 用 由 两 步 优 化 方法 得 到 的 转向 和 制 动 命 
令 来 控制 高 阶 车 辆 模型 跟踪 期 望 轨迹 

Stanford 的 Craig 等 在 二 自由 度 自行 车 模型 基础 上 提出 了 仿 射 力矩 输入 模型 
( Affine Force Input) ， 并 将 横 摆 角速度 和 轮胎 侧 偏 角 约束 以 包 线 的 形式 纳 和 人 控 
制 器 (如 图 1.10 所 示 )， 使 车 辆 能 够 在 极限 情况 下 完成 轨迹 跟踪 ， 实 现 车 辆 





图 1.9 前 方 有 障碍 (车 辆 或 其 他 
障碍 ) 的 换 道 行驶 情形 


横 摆 角速度 /[( ) +s] 
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侧 偏 角 / Co 
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性 能 的 最 大 化 利用 。 文 献 [55] 则 在 动力 学 建 模 时 考虑 了 车 轮转 角 函 数 随 
速度 的 变化 ， 提 高 了 车 辆 在 高 速 行驶 情况 下 的 控制 精度 。 

(4) 微分 平坦 方法 在 非 线 性 运动 控制 模型 简化 中 的 应 用 

微分 平坦 在 无 人 (空中 或 地 面 ) 车 辆 或 者 机 器 人 轨迹 跟踪 控制 中 得 到 了 
一 定 的 关注 。 微 分 平坦 是 一 个 优化 控制 问题 。 从 理论 上 分 析 ， 它 与 全 状态 反 
馈线 性 优化 是 等 价 的 。 微 分 平坦 系统 的 定义 : 如 果 一 个 系统 能 找到 平坦 输 
出 ， 使 系统 的 状态 和 输入 都 可 以 用 平坦 输出 及 其 有 限 阶 导数 表示 ， 则 该 系统 
被 称 为 平坦 系统 ?05 。 平 坦 系 统 的 一 个 好 处 就 是 能 将 非 线 性 系统 的 微分 约 
束 映射 为 平滑 几何 约束 ， 有 效 减 少 优化 空间 的 维 数 。 对 于 车 辆 非 线性 系统 ， 
利用 微分 平坦 输出 ， 能 将 同时 满足 运动 学 约束 (速度 ) 和 动力 学 约束 (如 
速度 ) 的 Kinodynamic 规划 与 控制 问题 简化 为 计算 光滑 轨迹 的 几何 规划 与 控 
制 问 题 。 

另外， 微分 平坦 简化 模型 轨迹 跟踪 的 控制 稳定 性 非常 重要 。 这 是 深 动 优化 
评价 的 基础 。 文 献 [51] 完成 了 固定 加 速度 横 摆 动力 学 模型 在 跟踪 固定 曲率 
轨迹 的 控制 稳定 性 分 析 ， 但 没有 涉及 时 变 加 速度 模型 及 时 变 曲率 轨迹 。 

(5) 侧 倾 约束 对 轨迹 跟踪 控制 的 影响 

目前 已 经 有 商用 化 的 车 辆 (特别 是 挂车 及 高 档 越野 轿车 ) 防 侧 倾 控制 系 
统 。 其 原理 主要 是 综合 前 轮 偏 角 与 差 动 制 动 方法 输入 控制 量 、 主 动 甚 架 控制 技 
术 调 整 重心 位 置 、 发 动机 扭矩 控制 与 ABS 系统 结合 使 用 等 方法 达到 防 侧 倾 的 
目的 。 对 于 在 越野 环境 中 运行 的 车 辆 或 者 高 速 车 辆 紧急 避 险 转向 时 侧 倾 与 滑 移 
约束 相 结合 的 轨迹 跟踪 控制 稳定 性 分 析 是 非常 重要 的 。lagnemma ”采用 能 量 
成 形 控制 (Energy Shaping Control). 分 析 了 近似 成 倒立 摆 的 车 辆 模型 侧 倾 控制 
稳定 性 ， 借 鉴 了 倒立 摆 的 控制 稳定 性 分 析 (如 图 1.11 所 示 )。 虽然 忽略 了 横 
摆 滑 移 非 线性 ， 但 能 量 成 形 控制 稳定 性 方法 体现 了 较 好 的 易 用 性 
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图 1.11 轨迹 跟踪 时 车 辆 侧 倾 约束 倒立 摆 模 地 
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(6) 模型 预测 控制 算法 实时 性 

与 移动 机 器 人 不 同 ， 无 人 车 辆 处 于 高 速 运动 状态 ， 车 辆 不 可 能 停 下 来 或 者 
减速 等 竺 规划 或 控制 器 的 输出 结果 。 基 于 模型 预测 控制 的 轨迹 跟踪 控制 器 实时 
性 问题 一 方面 受到 所 采用 动力 学 模型 的 影响 ， 另 外 一 方面 则 是 自身 计算 方式 的 
影响 。 目前 模型 预测 控制 的 基本 思想 都 是 在 线 求解 序列 二 次 规划 问题 ， 而 当 系 
统 较为 复杂 或 者 约束 量 比较 多 时 ， 计 算 速 度 不 能 满足 实时 控制 的 需求 。 提 高 计 
算 速度 的 一 类 方法 是 将 二 次 规划 问题 完全 用 控制 量 表 示 ， 并 假定 在 某 一 段 控制 
时 域内 控制 量 不 变 。 这样 可 以 大 大 减少 优化 过 程 中 的 变量 ， 从 而 提高 计算 速 
ES. 另外 一 种 重要 的 方法 被 称 为 “ 暖 启 动 ””， 即 在 每 一 步 开 始 计算 的 时 
候 根 据 上 一 步 的 预测 结果 进行 初始 化 。 如 果 运 用 得 当 的 话 ， 这 种 方法 将 会 使 总 
计算 次 数 减少 为 原来 的 173。 

随 着 模型 预测 控制 算法 在 各 领域 的 应 用 愈加 广泛 ， 有 学 者 将 前 期 的 一 些 研 
究 成 果 进 行 集成 ， 形 成 了 方便 研究 者 使 用 的 工具 箱 。Fast MPC 是 目前 应 用 较 
为 普遍 的 快速 求解 工具 箱 。 它 的 基本 思想 是 在 求解 锅 得 到 符合 目标 精度 的 结果 
后 就 提前 结束 迭代 过 程 ， 即 不 再 要 求 每 一 步 计算 过 程 都 得 到 最 优 解 。 实 验 结果 
AU]. 应 用 该 工具 箱 处 理 含有 450 个 变量 和 1284 个 约束 条 件 的 复杂 系统 时 ， 
可 以 达到 200Hz 的 计算 速度 ”。 多 参数 规划 工具 箱 (Multi-Parametric Toolbox, 
MPT) 是 一 类 快速 求解 线性 规划 或 者 二 次 规划 问题 的 工具 箱 。 其 基本 原理 是 通 
过 应 用 多 参数 规划 方法 得 到 约束 最 优 控 制 序列 与 状态 之 间 的 显 式 函 数 关 系 ， 减 
少 规划 问题 求解 的 工作 量 " 

(7) 轨迹 跟踪 算法 及 存在 的 问题 小 结 

基于 人 模 击 预测 控制 的 轨迹 跟踪 算法 已 经 有 了 一 定 的 研究 ， 对 车 辆 动力 学 的 
建 模 已 经 比较 成 熟 ， 仿 真 和 实验 都 验证 了 算法 的 可 行 性 与 可 靠 性 。 主 要 突破 
点 为 : 


D 对 动力 学 模型 合理 简化 ， 减 少 控 制 融 的 在 线 计算 量 ， 提 高 控制 系统 的 
实时 性 
D 对 高 速 情况 下 的 动力 学 模型 进行 分 析 ， 提 高 控制 系统 在 车 辆 高 速 行驶 


时 的 控制 精度 

高 速 车 辆 在 危险 障 但 规避 时 的 运动 轨迹 跟踪 控制 过 程 中 ， 为 保证 控制 算法 
满 趾 非 线 性 约束 和 实时 性 要 求 ， 采 用 了 车 辆 动力 学 模型 的 降 维 方法 ; 但 目前 设 
计 的 运动 轨迹 跟踪 控制 器 中 大 部 采用 数值 优化 解 算 非 线性 轮胎 模型 ， 模 型 的 复 
杂 性 增加 了 计算 负担 

目 线 性 系统 的 征 分 平坦 输出 特性 能 有 效 地 将 运动 轨迹 的 动力 学 和 运动 学 
Kinodynamie 约束 映射 为 几何 平 潜 约 更 ， 简 化 了 非 线性 系统 规划 与 控制 计算 复 
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杂 度 ， 同 时 将 车 辆 前 轮 附 近 的 振动 中 心 位 置 属性 作为 非 线性 自行 车 模型 的 平坦 
输出 ， 在 高 速 滑动 漂移 的 运动 轨迹 跟踪 控制 中 有 很 好 的 研究 价值 。 

时 变 加 速度 车 辆 动力 学 模型 ( 横 摆 动力 学 及 其 他 约束 等 效 模型 ) 跟踪 时 
变 曲率 轨迹 的 控制 稳定 性 分 析 尚 待 突破 。 


1.4 本 书 内 容 与 结构 说 明 


虽然 前 面 列 出 了 地 面 无 人 驾驶 车 辆 运动 规划 与 控制 中 的 一 系列 挑战 性 问 
题 ， 但 我 们 在 研究 过 程 中 发 现 模型 预测 控制 方法 能 够 充分 考虑 这 些 挑战 性 问题 
中 涉及 的 车 辆 模型 约束 问题 ， 并 进行 了 一 些 初步 的 探索 "本 书 对 
这 些 初步 的 研究 成 果 进 行 了 系统 的 总 结 ， 一 方面 作为 团队 进一步 深入 研究 的 基 
础 材料 ， 另 一 方面 也 可 以 作为 模型 预测 控制 理论 应 用 研究 的 参考 资料 。 另 外 ， 
鉴于 模型 预测 控制 理论 的 数学 抽象 性 较 强 ， 入 门 和 理解 需要 较 长 时 间 ， 为 了 方 
便 读 者 充分 理解 本 书 内 容 ， 书 中 给 出 了 所 有 算法 Matlab 代码 ， 同 时 仿真 实例 
也 都 列 出 了 详细 步骤 。 

本 书 假定 无 人 驾驶 车 辆 的 感知 系统 能 够 对 行驶 环境 进行 充分 的 理解 ， 能 够 
得 到 障碍 物 、 道 路 条 件 等 环境 信息 ， 同 时 具备 全 局 路 径 规划 能 力 ; 车 辆 平台 有 具 
备 自动 驾驶 控制 能 力 ， 横 向 与 纵向 控制 系统 能 够 控制 车 辆 转向 与 速度 ， 并 且 能 
向 控制 系统 反馈 车 辆 外 部 和 内 部 状态 信息 。 

第 2 ~7 章 的 内 容 如 下 : 

第 2 章 介绍 用 于 地 面 无 人 驾驶 车 辆 运动 规划 与 控制 的 车 辆 运动 学 模型 和 动 
力学 模型 。 车 辆 类 型 选择 前 轮转 向 的 乘 用 汽车 。 介 绍 了 车 辆 运动 学 建 模 过 程 并 
对 模型 进行 了 验证 ; 介绍 了 侧 向 动力 学 模型 和 轮胎 模型 ， 并 给 出 了 Matlab {È 
码 实例 验证 。 

第 3 章 首先 阐述 模型 预测 控制 基本 概念 ， 扩 展 到 两 类 常用 的 模型 预测 控制 
方法 ， 并 以 工程 实例 进一步 加 深 对 这 种 方法 的 认识 。 然 后 分 别 对 线性 模型 预测 
控制 和 非 线 性 模型 预测 控制 进行 介绍 ， 并 对 仿真 源 代码 进行 分 析 ， 让 读者 能 够 
尽快 熟悉 并 编写 自己 的 控制 算法 。 

第 4 章 从 运动 学 模型 角度 出 发 ， 开 发 具有 轨迹 跟踪 功能 的 控制 器 ， 在 给 定 
期 望 轨 迹 点 的 情况 下 ， 完 成 对 期 望 轨迹 的 跟踪 控制 ， 并 详细 说 明 搭 建 Simu- 
link/CarSim 联合 仿真 平台 的 过 程 ， 对 所 设计 的 控制 器 进行 仿 直 验证 。 本 章 将 
理论 与 实践 相 结 合 ， 在 介绍 完 轨迹 跟踪 控制 器 的 理论 算法 后 ， 通 过 实例 进一步 
加 深 对 理论 的 认 知 。 从 CarSim 软件 使 用 和 Matlab 代码 分 析 的 角度 ， 让 读者 对 
轨迹 跟踪 控制 的 设计 与 使 用 有 深入 的 理解 。 
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第 5 章 以 无 人 驾驶 车 辆 的 主动 转向 控制 为 应 用 背景 ， 建 立 以 车 辆 动力 学 模 
型 为 基础 的 模型 预测 控制 器 。 结 合 车 辆 高 速 运动 下 的 各 类 约束 ， 进 一 步 讨论 复 
杂 约 束 条 件 下 模型 预测 控制 的 求解 方法 ， 并 在 Simulink/CarSim 联合 仿真 平台 
上 验证 算法 的 有 效 性 

第 6 章 介绍 地 面 无 人 驾驶 车 辆 结合 路 径 规划 层 的 轨迹 跟踪 控制 器 。 车 辆 类 
型 选择 前 轮转 向 的 乘 用 汽车 。 首先 ， 介 绍 了 基于 车 辆 点 质量 模型 的 MPC 轨迹 
规划 控制 器 ， 规 划 出 满足 车 辆 动力 学 约束 并 实现 避 障 的 可 行 轨 迹 ， 并 给 出 了 相 
应 的 S 函数 。 其次， 介绍 了 在 车 辆 动力 学 模型 的 基础 上 的 MPC 路 径 跟 踪 控制 
器 ， 并 给 出 了 SS 函数 人 代码。 再次， 详细 介绍 了 运用 Simulink/CarSim 进行 联合 
仿真 的 实例 验证 

第 7 章 是 基于 模型 预测 控制 思想 设计 的 实际 车 辆 控制 中 运用 的 单 步 航 向 预 
估 控 制 方法 ,虽然 没 有 进行 优化 ， 但 在 考虑 模型 约束 的 基础 上 ， 结 合 PID 控制 
方法 ,在 单片机 程序 中 实现 了 无 人 和 驾驶 车 辆 航向 单 步 预测 ， 有 和 较 好 的 控制 
效果 
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驾驶 车 辆 运动 规划 与 控制 需要 通过 对 车 辆 运动 学 或 者 动力 学 系统 的 控 
Nes 从 第 1 章 的 介绍 可 以 看 出 ， 如 果 规 划 阶 段 能 够 考虑 车 辆 运动 学 和 动 
力学 约束 ， 那 么 运动 跟踪 控制 性 能 会 更 好 。 建 立 合 理 的 车 辆 系统 模型 不 仅 是 设 
计 模 型 预测 控制 器 的 前 提 ， 也 是 实现 车 辆 道路 跟踪 功能 的 基础 。 因 此 ， 在 建立 
模型 预测 控制 器 时 ， 必 须根 据 无 人 驾驶 车 辆 的 具体 行驶 工 况 ,通过 选取 合适 的 
控制 变量 ， 建 立 能 够 准确 描述 无 人 驾驶 车 辆 运动 关系 约束 的 运动 学 模型 和 描述 
动力 学 约束 的 动力 学 模型 。 

车 辆 在 地 面 运动 的 动力 学 过 程 是 非常 复杂 的 ， 为 了 尽量 准确 描述 车 辆 运 
动 ， 需 要 建立 复杂 的 微分 方程 组 ， 并 用 多 个 状态 变量 来 描述 其 运动 。 在 第 1 章 
已 经 讨论 到 ， 用 于 模型 预测 控制 的 模型 只 要 能 够 表现 出 车 辆 运动 学 与 动力 学 约 
束 ， 就 可 以 使 模型 预测 控制 器 实现 预定 控制 目的 。 特 别 是 在 规划 阶段 ， 为 了 保 
证 规划 算法 的 实时 性 ， 约 东 简 化 和 近似 就 是 一 种 非常 重要 的 手段 ， 比 如 轮胎 靡 
擦 圆 约束 和 点 质量 模型 。 因 此 ， 过 于 复杂 的 模型 并 不 是 研究 的 重点 。 本 章 从 无 
人 驾驶 车 辆 路 径 重 规划 和 道路 跟踪 控制 的 角度 对 车 辆 系统 进行 建 模 ， 建 立 能 够 
尽量 准确 反映 车 辆 运动 特性 ， 并 且 有 利于 模型 预测 控制 器 设计 的 简化 车 辆 运动 
学 模型 和 动力 学 模型 。 在 此 基础 上 ， 对 所 建立 的 模型 进行 验证 


2.1 车 辆 运动 学 建 模 及 验证 
运动 学 是 从 几何 学 的 角度 研究 物体 的 运动 规律 ， 包 括 物体 在 空间 的 位 置 、 
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速度 等 随时 间 而 产生 的 变化 。 在 车 辆 路 径 规划 算法 中 应 用 运动 学 模型 ， 可 以 使 
规划 出 的 路 径 切 实 可 行 ， 并 满足 行驶 过 程 中 的 运动 学 几何 约束 ; 同时 ， 在 良好 
路 面 的 低速 行驶 工 况 下 ， 一 般 不 需要 考虑 车 辆 稳定 性 控制 等 动力 学 问题 。 基 于 
运动 学 模型 设计 的 路 径 跟 踪 控 制 器 具备 可 靠 的 控制 性 能 。 

2.1.1 车 辆 运动 学 建 模 

车 辆 转向 运动 模型 如 图 2. 1 所 示 。 在 惯性 坐标 系 OXY F, (X,, Y) 和 
CX... Y. 分 别 为 车 辆 后 轴 和 前 轴 轴 心 的 坐标 ，e 79 AE PROS pA RS (航向 角 )， 





0, 为 前 轮 偏 角 ，w 为 车 辆 后 轴 中 心 速度 ,wi 为 车 辆 前 轴 中 心 速 度 ，! 为 轴 距 
(注意 ， 变 量 下 标 了 上 代表 front 前 , 工 代 表 rear 一 一 后 ， 下 同 )。 





图 2.2 所 示 为 车 辆 转向 过 程 示 意图 ，R 为 后 轮转 向 半径 ，P 为 车 辆 的 瞬时 
转动 中 心 ，W 为 车 辆 后 轴 轴 心 ，N 为 前 轴 轴 心 。 此 处 假设 转向 过 程 中 车 辆 质心 
侧 偏 角 保 持 不 变 ， 即 车 辆 瞬时 转向 半径 与 道路 曲率 半径 相同 。 











图 2.1 车 辆 运动 模型 图 2.2 车 辆 前 轮转 向 示 意图 
在 后 轴 行 驶 轴 心 (X,，》.) 处 ， 速 度 为 : 
p = X coso + Y sing (2. 1 ) 


前 、 后 轴 的 运动 学 约束 为 : 


ny ius ー Y,cos( e +8) 20 (2.2) 
Y sing - Y coso = 0 
由 式 (2.1) 和 式 (2.2) 联合 可 得 : 
に = V Coso (2.3) 
y, = vsing 


根据 前 后 轮 的 几何 关系 可 得 : 
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に = X, + lcosp (2.4) 
Y, = Y, + lsing 
将 式 (2.3) 和 式 (2.4) 代入 式 (2.2) ， 可 解 得 横 摆 角 速度 为 : 

u = te (2.5) 


l 
式 中 、o 为 车 辆 横 摆 角速度 ; 同时 ， 由 w MER w 可 得 到 转向 半径 R Ae 
偏 角 Or: 








R = v/v 

| (2.6) 
ô = arctan( [/R) 

由 式 (2.3) 和 式 (2.5) 可 得 到 车 辆 运动 学 模型 为 : 
x coso 
3 -| sing | (2.7) 
tanó,/l 
e 
该 模型 可 被 进一步 表示 为 更 为 一 般 的 形式 : 
Es. = fin (Ekin sin ) (2.8) 


Hop, RER fin = [X Y. p], 挫 制 量 un = [w，5] 。 在 无 人 驾驶 车 
辆 的 路 径 跟踪 控制 过 程 中 ， 往 往 希 望 以 [v,，w] 作为 控制 量 , 将 式 (2.5) 
代入 式 (2.7) 中 ， 该 车 辆 运动 学 模型 可 以 被 转换 为 如 下 形式 : 


X, COS の 0 
Y | = ie h * 中 (2.9) 
e 0 1 

2.1.2 车 辆 运动 学 模型 验证 


为 了 验证 所 建立 的 车 辆 运动 学 模型 ， 在 Matlab/Simulink 环境 中 搭建 该 运 
动 学 模型 ， 在 相同 的 输入 条 件 下 与 CarSim 中 所 建立 的 整 车 模型 进行 对 比分 析 。 
其 中 ， 相 同 的 输入 条 件 是 指 前 轮 偏 角 与 车 速 随 时 间 的 变化 历程 相同 ， 输 出 均 为 
车 辆 位 置 和 航向 。 

车 辆 基本 参数 设置 为 : 

轴 距 ;1 =2.7 m; 

初始 状态 : £,, =[0, 0, 0]。 

用 于 模型 验证 的 输入 信号 随时 间 的 变化 历程 如 图 2.3 所 示 ， 其 中 图 2.3 (a) 
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为 车 速 随时 间 的 变化 历程 ,图 2.3 (b) 为 前 轮 偏 角 随时 间 的 变化 历程 。 上 述 所 
建立 的 运动 学 模型 和 CarSim 整 车 模型 的 输出 信号 如 图 2.4 所 示 。 其 中 图 2.4 
(a) 为 两 者 在 给 定 的 输入 条 件 下 输出 的 位 置信 息 对 比 ， 图 2.4 (b) 为 两 者 的 
航 回 角 信 息 对 比 。 如果 对 CarSim 和 Simulink 的 联合 使 用 方法 不 熟悉 ， 则 可 在 
看 完 第 4 章 的 详细 仿真 步骤 说 明 后 ， 再 完成 模型 的 验证 。 





8 
6 
* s S 4 
ご - 
E き 
4 
: き 
g 
3 3 
-4 
^0 10 20 30 40 50 0 10 20 30 40 50 
有 时间 i/s 时 间 i/s 
(a) (b) 
图 2.3 模型 验证 的 输入 信和 号 
(a) 车 速 随 时 间 的 变化 历程 ; (b) 前 轮 偏 角 随时 间 的 变化 历程 
150 一 一 
CarSim 模 型 输出 CarSim 模 型 输出 
i Sae 运动 学 模型 输出 运动 学 模型 输出 
E > E 
= 90 ー 
1E Y 
— E 
m 60 Pc 
x zm 
30 
{| 
0 40 80 120 160 0 10 20 30 40 50 
Bie] 4e bc / m 时 间 //s 
(a) (b) 


I^ 2.4 pi DET hii g は 対 比 
(a) 车 辆 位 置 对 比 ; (b) 车 辆 航向 角 对 比 


从 图 2.4 中 的 结果 对 比 可 以 看 出 ， 在 相同 的 速度 和 前 轮 偏 角 输入 下 ， 运 动 
学 模型 的 车 辆 位 置 和 航向 角 与 CarSim 和 输出 的 结果 非常 吻合 。 也 就 是 说 ， 
式 (2.8) 表示 的 模型 能 够 较 好 地 反映 车 辆 行驶 时 的 运动 学 特性 。 在 后 续 规 划 
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算法 以 及 低速 轨迹 跟踪 控制 算法 中 ， 将 在 仿真 实验 中 使 用 该 模型 。 


2.2 车 辆 动力 学 建 模 及 验证 


车 辆 整 车 动力 学 模型 一 般 包括 用 于 分 析 车 辆 平顺 性 的 质量 - 弹簧 -阻尼 
模型 和 分 析 车 辆 操纵 稳定 性 的 车 辆 - 轮胎 模型 。 两 者 研究 的 侧重 点 不 同 。 平 
顺 性 分 析 的 重点 是 车 辆 的 悬 架 特性 ， 而 车 辆 操纵 稳定 性 分 析 的 重点 是 车 辆 纵 
向 及 侧 向 动力 学 特性 。 本 书 主要 研究 目标 是 使 车 辆 快速 而 稳定 地 跟踪 期 望 路 
径 ， 属 于 车 辆 操纵 稳定 性 问题 ， 因 此 对 于 悬 架 特性 不 做 深入 探究 ; 同时 ， 本 
书 所 建立 的 动力 学 模型 主要 是 作为 模型 预测 控制 器 中 的 预测 模型 使 用 ， 需 要 
在 较为 准确 地 描述 车 辆 动力 学 过 程 的 基础 上 尽 可 能 进行 简化 ， 以 减少 控制 算 
法 的 计算 量 。 

综合 上 述 分 析 ， 在 进行 车 辆 动力 学 建 模 时 ， 进 行 以 下 理想 化 的 假设 : 

① 假设 无 人 驾驶 车 辆 在 平坦 路 面 上 行驶， 忽略 车 辆 垂 向 运动 。 

② 悬 架 系 统 及 车 辆 是 刚性 的 ， 忽 略 悬 架 运 动 及 其 对 耦合 关系 的 影响 

© 只 考虑 纯 侧 偏 轮 胎 特性 ， 忽 略 轮胎 力 的 纵横 向 耦合 关系 。 

④ 用 单轨 模型 来 描述 车 辆 运动 ， 不 考虑 载荷 的 左右 转移 。 

© 假设 车 辆 行驶 速度 变化 缓慢 ， 忽 略 前 后 轴 的 载荷 转移 。 

(6) 忽略 纵向 和 横向 空气 动力 学 。 


2.2.1 车 辆 单轨 模型 


基于 以 上 6 点 理想 假设 ,平面 运动 车 辆 只 有 具有 3 个 方向 的 运动 ， 即 纵向 、 横 
向 和 横 摆 运动 。 设 定 车 辆 为 前 轮 驱 动 。 满 足以 上 设 定 的 平面 运动 车 辆 单轨 模型 如 
图 2.5 所 示 。 其 中 ， 坐 标 系 oxyz 为 固定 于 车 身 的 车 辆 坐标 系 。xoz 处 于 车 辆 左右 
对 称 的 平面 内 ， 车 辆 质心 所 在 点 为 坐标 原点 o，x 轴 为 沿 车 辆 纵 轴 ，y 轴 与 车 辆 
纵 轴 方向 垂直 ， 而 z 轴 满足 右手 法 则 ， 垂 直 于 voy 且 向 上 。 坐 标 系 OXY 为 固定 于 
地 面 的 惯性 坐标 系 ， 也 满足 右手 法 则 。 图 2. 5 中 关于 轮胎 受 力 定义 如 下 : 

Fi, Fo: 前 、 后 轮胎 受到 的 纵向 力 。 

が が: 前 、 后 轮胎 受到 的 侧 向 力 。 

Fa, Fa: 前 、 后 轮胎 受到 的 x 方向 的 力 

F,, Fa: 前 、 后 轮胎 受到 的 y 方 向 的 力 。 
根据 牛顿 第 二 定律 ， 分 别 得 沿 x 轴 、y 轴 和 绕 : 轴 的 受 力 平衡 方程 。 
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图 2.5 车 辆 单轨 模型 


在 x 轴 方 向 上 : 


mx = myg *2F,, +2F, (2. 10) 

在 y 箇 方 向上 : 
my =- mxo +2F,, +2F,, (2.11) 

Zé z liri] E: 
Là i WF (2.12) 


式 中 、z, b 分 别 为 质心 到 前 、 后 轴 的 距离 ，m 为 车 辆 整备 质量 ，/. 为 车 辆 绕 z 
轴 的 转动 惯量 
轮胎 在 * 方向 和 > 方向 上 受到 的 合力 与 纵 、 侧 向 力 的 转换 关系 如 下 : 


F „ = F,cosó, - F „sinô, (2. 13) 
F., = F,cosó, - F „sinô, (2.14) 
F,, = F,sinó, + F ,cosó, (2.15) 
F „ = F,sinó, + F,,cosó, (2.16) 


轮胎 的 纵向 力 、 侧 向 力 可 以 表示 为 轮胎 侧 偏 角 、 滑 移 率 、 路 面 摩擦 系数 和 
if p] 4c dur E 2 CTI L AS PRA: 
F, = filas pE.) (2.17) 
F, - f. (a su, F.) (2.18) 
wh, a 5660 fü, s HAKK, a 为 路 面 摩 控 系数 ， 瓦 . 为 轮胎 所 受到 的 
ifr [6] 4 for. 
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轮胎 的 侧 偏 角 a 可 以 由 几何 关系 计算 得 到 : 
a = tan! à (2. 19) 
式 中 , v. fl v, Je E DE], DA TRI RO SERE, TAL HI A b 3873 EE v, 和 v, 
表示 : 
U, = U,sind + v,cosô (2.20) 
の = U,cosÓ — U,sinó (2.21) 
式 中 , 8 为 轮胎 偏转 角 。 
轮胎 的 速度 往往 难以 直接 获取 ， 一般 可 以 通过 车 辆 速度 计算 得 到 。 根 据 
图 2.5 中 的 速度 关系 可 以 推导 出 以 下 转换 关系 : 


v -y*ap v, =y -bọ (2.22) 
U= v, =à (2.23) 
轮胎 在 地 面 上 的 滑 移 率 可 以 由 以 下 算式 计算 : 
hi q1(9 » no, 0» 0) 
A J Ee (2.24) 


1 -= (v < rw,,w, = 0) 


式 中 ，r 为 车 轮 半 径 ，w, 为 车 轮 旋转 角速度 。 

假设 车 辆 行驶 速度 变化 缓慢 ， 忽 略 前 后 轴 的 载 衔 转移 ， 可 以 通过 以 下 算式 
计算 得 到 车 辆 前 、 后 轮胎 所 受到 的 垂 向 载荷 : 
bmg 





F, - i.e (2.25) 
F, = Sas (2.26) 

最 后 ， 考 虑 车 身 坐标 系 与 惯性 坐标 系 之 间 的 转换 关系 ， 可 得 : 
Y = xsing + ycosp (2.27) 
X = xcose - ysing (2.28) 


结合 式 (2.10) ~ 式 (2.28) ， 可 以 得 到 车 辆 非 线性 动力 学 模型 。 通 过 算式 
间 的 代 换 ， 除 了 路 面 摩擦 系数 jw 和 滑 移 率 * 外 ， 其 他 参数 都 可 以 由 车 辆 状态 信 
息 计 算得 到 。 路 面 摩擦 系数 jy 为 道路 固有 信息 ， 给 定 道路 条 件 后 就 能 获取 。 把 
HER s 作为 系统 的 控制 量 ， 将 有 效 改 善 车 辆 在 低 附着 路 面 的 行驶 性 能 ， 但 对 
于 滑 移 率 的 控制 本 身 就 是 一 个 复杂 的 控制 问题 。 因 此 ， 假 设 被 控 车 辆 具备 良好 
的 防 抱 死 制 动 系统 (ABS)， 滑 移 率 始终 保持 在 最 佳 工作 点 ， 将 系统 描述 为 以 
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O 
下 状态 空间 表达 式 : 


Em = Fas Ei Hays) 
Tas = has CE as) 
在 该 系统 中 ， 状 态 量 选取 为 &,, -[y, x, e, e, Y, X] ， 控 制 量 选取 为 
us =6:( 仅 考虑 前 轮转 向 车 辆 ，6, 视 为 0) ， 输 出 量 选 取 为 yu [o, Y]. 
在 实际 的 控制 过 程 中 ， 路 面 摩擦 系数 人 和 滑 移 率 * 视 为 已 知 量 ， 该 模型 即 模型 
预测 控制 咒 中 预测 模型 的 基础 


2.2.2 轮胎 模型 


在 进行 车 辆 动力 学 仿真 时 ， 轮 胎 所 受 的 垂直 力 、 纵 向 力 、 侧 向 力 和 回 正 
力矩 对 汽车 的 平顺 性 、 操 纵 稳 定性 和 安全 性 起 着 重要 作用 。 由 于 轮胎 结构 复 
杂 ， 动 力学 性 能 呈 非 线性 ,选择 符合 实际 又 便于 使 用 的 轮胎 模型 是 建立 车 辆 
模型 并 进行 动力 学 仿真 的 关键 。 主 要 的 轮胎 模型 可 以 分 为 理论 轮胎 模型 、 经 
验 轮胎 模型 和 物理 轮胎 模型 等 。 其 中 ，Pacejka 提出 的 以 魔术 公式 (Magic 
Formula, MF) 为 基础 的 经 验 轮 胎 模 型 ， 运 用 三 角 函 数 的 组 合 公 式 拟 合 轮胎 
试验 数据 ， 描 述 轮胎 的 纵向 力 FI， 侧 向 力 F.， 回 正 力矩 M.， 翻 转 力矩 M,, 
UH 7] 58. M, 与 侧 偏 角 a， 滑 移 率 s 之 间 的 定量 关系 ， 以 及 纵向 力 、 侧 向 力 的 
联合 作用 工 况 ,能 够 表达 不 同 驱 动情 况 时 的 轮胎 特性 ”。 魔术 公式 的 一 般 
表达 式 为 : 

Y(x) = Dsin| Carctan| Bx — E( Bx — arctan( Bx))|| (2.30) 
式 中 , ALB, C, 也 依次 由 轮胎 的 垂直 载荷 和 外 倾角 确定 ; 了 为 输出 变量 ， 
可 以 是 纵向 力 Fi 或 侧 向 力 F, 或 者 回 正 力矩 ; x 为 输入 变量 ， 在 不 同 的 情况 下 
分 别 表示 轮胎 的 侧 偏 角 a eX A IEEE s; 为 刚度 因子 ; C 为 形状 因子 ; D 
为 峰值 因子 ; E 为 曲率 因子 

在 实际 应 用 中 ， 由 于 帘 布 层 转 癌 效应 、 侧 偏 力 作用 或 滚动 阻力 会 引起 偶 
移 ， 通 常 还 会 引入 垂直 偶 移 S, 和 水 平 偏 移 $S，( 见 图 2.6)， 

Pacejka 轮胎 模型 认为 ， 轮 胎 在 乖 直 、 侧 面 方向 上 是 线性 的 ,阻尼 为 常量 。 
这 在 侧 向 加 速度 &, m0. 4 m, EE ON a ff 和 5 的 情景 下 对 常规 轮胎 具有 很 高 的 
拟 合 精度 。 此 外 ， 魔 术 公 式 具 有 较 好 的 健壮 性 ， 在 极限 值 以 外 一 定 程度 内 仍 可 
使 用 ， 可 以 对 有 限 工 况 进 行 外 推 且 具有 较 好 的 置信 和 度 

利用 魔术 公式 计算 轮胎 的 纵 癌 力 及 侧 向 力 的 关系 式 分 别 介绍 如 下 : 

(1) 轮胎 纵向 力 计算 方法 

F, = Dsin| Carctan( Bx, — E( Bx, — arctan( Bx, ) ) ) | * S, (2.31) 


(2. 29) 
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输入 变量 输出 变量 


纵向 滑 移 率 s 
HJ FIN] 
侧 偏 角 a [rad] 侧 向 力 FIN] 
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图 2.6 基于 魔术 公式 的 轮胎 模型 的 输入 和 输出 变量 


式 中 ,x =s +S., s 为 纵向 滑 移 率 〈 负 值 出 现在 制 动 态 ，- 100 表示 车 轮 抱 
死 ) S, 为 曲线 的 水 平方 向 漂移 ，$, 为 曲线 的 垂直 方向 漂移 。 

C 为 曲线 的 形状 因子 ，C = Bu。 

D 为 曲线 瞻 因 子 ， 表 示 曲 线 的 最 大 值 , D = BF + B,F.， 其 中 下 .为 轮胎 受 
FI) AY HE [6] RTT o 

B 为 刚度 因子 ，B = (BE +B,F,) xe™™™/(C xD) 

为 曲线 的 曲率 因子 ， 表 示 曲 线 最 大 值 附近 的 形状 ,EE = BLF: +B,F, +B, 

S, 为 曲线 的 水 平方 向 漂移 : S,-B,F. B, 

S, 为 曲线 的 垂直 方向 漂移 : S, -0. 

在 纯 滑 移 率 作用 下 轮胎 的 纵向 力 如 图 2.7 所 示 。 









纵向 力 ( 纯 纵 滑 ) 
8 000 T T T T T T T 
72:000. enero MMERMEEI a MEINEM MEE" í ads — — 
6 000 La ee koa ÁÀ] E MINE : kes ; ーー 


S000 ト 上 ーーー os : occid : TEN RETE : PEN 


3418] 7] /N 





=20 —— «15 — «1b -5 0 5 10 I5 20 
纵向 滑 移 率 


图 2.7  Pacejka' 89 轮胎 纵 问 力 示 例 


5rjs.cn 000000 


© 第 2 章 车 辆 运动 学 与 动力 学 建 模 





(2) 轮胎 侧 向 力 计算 
F. = Dsin| Carctan( Bx - E( Bx - arctan(Br)))] + S, (2.32) 

式 中 ,x =a+5,，a 为 轮胎 侧 偏 角 ，S, 为 曲线 的 水 平方 向 漂移 ，S, 为 曲线 的 
重 直 方向 漂移 。 

C 为 曲线 的 形状 因子 ，C =A: 

D 为 曲线 蔷 因 子 ， 表 示 曲 线 的 最 大 值 ， 刀 = 怀 忆 + AsF.。 

B 为 刚度 因子 ，B = A;sin(2arctan z) x(1-A,l yl )/(CxD), 其 中 yy 为 
轮胎 外 倾角 

E 为 曲线 的 曲率 因子 , E-AF. +4. 

S, 为 曲线 的 水 平方 向 漂移 : S, =A F, +4io+487y。 

S, 为 曲线 的 垂直 方向 漂移 : S, =A Fy +41F. A5. 

在 纯 侧 俩 角 作 用 下 轮胎 的 侧 向 力 如 网 2. 8 所 示 


侧 向 力 ( 纯 侧 偏 ) 





側 向 力 /N 





-8 000 i i 一 i i i 
-8 -7 -6 -S -4 -3 -2 -| 0 ] 2 3 4 5 6 7 8 


側 備 角 /( ) 


[42.8  Pacejka' 89 轮胎 侧身 力 示例 


(3) 轮胎 回 正 力 甜 计算 
M, = Dsin| Carctan( Bx, — E( Bx, — arctan(Bx,) )) ] +8, (2.33) 
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此 时 的 x 为 回 正 力矩 计算 组 合 自 变量 : x, =(a+S) ，a 为 侧 偏 角 。 

C 为 曲线 形状 因子 ， 回 正 力矩 计算 时 取 C, 值 : C = Co。 

/ 为 诬 因 子 ， 表 示 曲 线 的 最 大 值 : D = CIF: «C... 

BCD 为 回 正 力矩 零点 处 的 扭转 刚度 : BCD 2 (CyFi +CsF.) x(Y- C | yl x 
-GF. 

B 为 刚度 因子 : B-BCD/(CxD). 

S, 为 曲线 的 水 平方 向 漂移 : S, = Cay + CuF.+ Co 

S, 为 曲线 的 垂直 方向 漂移 : S, =y (CuF: + CF) € CF. C, 

E 为 曲线 曲率 因子 , ES (CFL € CF. *C,) x(1- C, 1 yl ). 

在 纯 侧 偏 角 作 用 下 轮胎 的 回 正 力矩 如 图 2. 9 所 示 。 


EEJ H ( 纯 侧 偏 ) 


回 正 力矩 / (Nm) 
e 





p —o—x M —4 o3 3 | d 23 d 4 3$ O0 T B 
fifa fa / C» 
[8] 2.9  Pacejka' 89 轮胎 回 正 力矩 示例 


(4) Matlab m 文件 实现 


在 轮胎 纵向 力 和 侧 向 力 计算 式 中 ， 系 数 a0 - 13, 00 - 010, © -c17 根据 
轮胎 实验 数据 拟 合 得 到 。 本 文 所 采用 的 系数 如 表 2 -1 所 示 。 
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表 2 -1 魔术 公式 参数 的 取 值 
参数 名 称 Siki | 参数 名 称 。 ”参数 取 什 
a0 . 65 | b0 2.372 72 2. 340 00 
al —-9. 46 $ 1.4950 

1 490 6. 416 654 
130 Y -3.574 03 
276 — 0. 087 737 
0. 005 01 0. 088 6 0. 098 410 
- 0. 021 03 0. 004 02 0. 002 769 9 
0. 77394 0. 061 5 J —0. 000 115 1 

0. 002 289 0 1.2 0. 100 0 

0. 013 442 0. 029 9 -1. 333 29 
0. 003 70 -0. 176 : 0. 025 501 
19. 1656 - 0. 023 57 
1.213 56 $ 0. 030 27 
6. 262 06 $ - 0. 064 7 

0.021 1329 
0. 894 69 

— 0. 099 443 
- 3. 336 941 

















Matlab 仿真 程序 如 下 〈 注 意 在 下 面 及 本 书后 面 的 代码 中 ， 为 保证 与 下 载 的 
源 代 码 一 致 ， 说 明 与 符号 均 采 用 正体 ) : 





る Pacejka'89 轮胎 模型 认为 轮胎 在 垂直 、 侧 向 方向 上 是 线性 的 ,阻尼 为 


& 这 在 侧 向 加 速度 常见 范围 <0.4g . 侧 偏 角 <5° 的 情景 下 对 常规 轮胎 具有 
很 高 的 拟 合 精度 
S 这 个 算式 里 没有 考虑 地 面 摩擦 系数 的 影响 


る る 


Fz-5; t 垂下 載荷 单位 :kN 

% Longitudinal Force (pure longitudinal slip) 

% input:slip ratio 消 移 率 s 

s-linspace(-20,20,40); € -20-20 插值 40 人, 生成 消 移 率 s 横 
坐标 

Qo ekkki Longitudinal coefficients 纵向 系数 txk 多 
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b0 =2 .37272 ; 
bl =-9.46; 
b2 =1490: 
b3 2130; 
b4 -276; 
b5 20.0886; 
b6 0.00402; 
b7 =-0.0615: 
be 21.2; 
b9 20.0299; 
b10--0.176; 
So ホホ ポポ ネネ ポポ ポポ キキ ポポ parameters 参 数 skcke sk se ke zb ke nj sje obe obe sje ke ke bob besote 名 
Cx =b0 : s 曲线 形状 因子 
Dx-bl*Fz^2 «b2*Fz; を £k p 
BCDx = (b3 * Fz^2 +b4 * Fz) * exp(- b5 * Fz); 
S 35] 7] 2E ga tb RS ZA T] FI RE 


Bx = BCDx /(Cx * Dx); s 刚度 因子 

Shx-b9 * Fz + b10; S 曲线 的 水 平方 向 漂移 

kx=s+Shx; gs 输入 变量 XI 

Svx =0; gs 曲线 的 垂直 方向 漂移 

Ex-b6 *Fz^2 «b7 * Fz «b8; gs 曲线 的 曲率 因子 

FX=Dx*S1n(Cx* atan (Bx * kx- Ex * (Bx * kx - atan (Bx * kx))))+ 
Svx; S 纵向 力 的 计算 


figure (1); 


plot (s,Fx); S 绘制 纵向 力 曲 线 
grid on; S 绘制 栅 格 

set (gca, xlim',[-20 20]); s 设置 x 轴 范 围 
set (gca, *xtick',[-20:5:20]); g WT x 轴 间 隔 
set (gca, 'ylim'/,[-8000 8000]); s 设置 x d 
set (gca, 'ytick'/,[-8000:1000:8000]); $ 设置 x 轴 间隔 


xlabel (“纵向 滑 移 率 “); 
ylabel (“纵向 力 /(N)  ; 
title (“纵向 力 (el ATHE) ): 


% Lateral Force (pure side slip) 
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$ input :横向 侧 偏 是 侧 偏 角 alpha 
alpha-linspace(-8,8,16); 名 -8~8 插值 16 个 ,生成 侧 偏 角 横 


坐标 
ャ =0: s 外 倾角 , 设 为 零 
ポポ ポポ ポポ ポポ ポポ ポポ ポポ lateral coefficients wieetereleleereckeletereleleelelelelek $ 
a0 =1.65: 
al =-34: 
a2 -1250; 
a3 23036; 
a4 -12.8; 


a5 20.005071 ; 

a6 2-0.02103; 
a7 0.77394; 

a8 =0.0022890: 
a9 0.013442; 
a10 20.003709; 
all-219.1656; 
al2 21.213565; 
a13 =6.26206: 


£o 米 六 六 六 六 六 六 六 玉米 parameters ポポ ポポ ポポ ポポ ポポ ギ ポポ ポポ ポポ ポポ a ポ ポ $5 


Cy -a0; を 有 折线 形状 因子 
Dy =al *FzZ^2+a2 * FZ; gs mz A T 


BCDy -a3 * sin (2 * atan (Fz/a4)) * (1 - a5 * abs (r)); 

を rs] 7 FE Arb AS f] qs] du] JS 
By = BCDy /(Cy * Dy); る 刚度 因子 
曲线 的 水 平方 向 漂移 
输入 变星 x 
册 线 的 垂直 方向 漂移 
ill X ul DEP 


£o mehebbet lateral force formulation 


0 


Qoo 


Shy -a9 *Fz+a10+a8 *r; 


oe 


ky = alpha + Shy; 


oo 


Svy-all*Fz*r-c«al2*Fz-*al3; 


oe 


Ey-a6*Fz^242a7; 


Fy0 - Dy * sin (Cy * atan (By * ky - Ey * (By * ky - atan (By * ky))))« 
Svy ; 2 纵向 力 的 计算 
figure (2); 


plot (alpha,Fy0); 


5rjs.cn 000000 


无 人 驾驶 车 辆 模型 预测 控制 


grid 

set (gca, ‘xlim’, [ -8 8]); を 设置 x 轴 范 围 
set (gca, ‘xtick’, [ -8:1:8]); gs 设置 x 轴 间隔 
set (gca, 'ylim'/,[-8000 8000]); % 设置 x 轴 范围 


set (gca, "ytick,[-8000:1000:8000]): % 设置 x 轴 间隔 

xlabel (‘Mifi ^; 

ylabel (‘Mmj /(N) ^ ; 

title(“ 侧 向 力 ( 纯 侧 偏 ) ): 

$ $ Aligning Torque (pure side slip) 

gs input : 侧 偏 角 

等。 玉米 米 米 炒米 米 米 炒米 炒米 米 米 炒米 米 米 炒米 米 米 ALIGNING_COEFFICIENTS ホホ ポポ ポポ ポポ ポポ ポポ 名 

c0 2.34000; 

GT 21,4950; 

c2 26.416654; 

C3 2-3.57403; 

c4 =-0.087737: 

c5 20.098410; 

c6 20.0027699; 

c7 2-0.0001151; 

c8 20.1000; 

g9ie1.33329; 

ig 20.025501; 

GT オニ =0.02357 ょ 』 

c12 =0.03027: 

c13 =-0.0647: 

c14 =0.0211329: 

c15 =0.89469: 

c16 =-0.099443: 

cl7 =ー3 .336941 ; 

$ ネネ ポポ ポポ ポポ ポポ ポ parameters ポポ ポポ ポポ ポポ ポポ ボボ ポポ a ポポ ポポ a 名 

Cz-2c0; $ 曲线 形状 因子 

Dz-cl*Fz^24c2*Fz; $ ize 

BCDz = (c3*Fz^2 «c4 *Fz) * (1- c5 *abs(r)) * exp(-C5 * Fz); 
& 回 正 力矩 零点 处 的 扭转 
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———— ——————— —S 
刚度 
Bz - BCDz /(Cz * Dz); s 刚度 因子 
Shz =C11 * エ +C12 *Fz+ で 13 : S 曲线 的 水 平方 向 漂移 
kz=alpha+Shz; $ 输入 变量 x 


Svz-zr*(cl4*Fz^24cl5*Fz)e-cl6 *Fz+C17: 

$ 曲线 的 垂直 方向 漂移 
Ez -(C7 * FZ ”2+C8*FZ+C9)* (1- c10 *abs (r)); 

s 曲线 曲率 因子 
b sobxeeleletoioleceielereieieeielerek aligning torque formulation 
Mz0 -Dz * sin (Cz * atan (Bz * kz - Ez * (Bz * kz -atan (Bz * Kz ) ) ) )+ 

Svz; S 纵向 力 的 计算 

figure (3); 


plot (alpha,Mz0); 


grid 

set (gca,'xlim;[-8 8]); > 设置 x 轴 范围 
set (gca, 'xtick;[-8:1:8]); s 设置 x 轴 间 隔 
set (gca, “ylim; [ -80 80]); を 设置 x 轴 范 围 
set (gca, 'ytick;[-80:10:80]); る 设置 x 轴 间隔 


xlabel (Mifi); 
ylabel.('["1E JJXB /(N) ^) 
title(“ 回 正 力矩 ( 纯 侧 偏 ) ) ; 


2.2.3 小 角度 假设 下 的 车 辆 动力 学 模型 


通过 将 式 (2.29) 和 轮胎 模型 结合 ， 可 以 发 现 建立 的 非 线 性 状态 空间 表 
达 式 相对 于 模型 预测 控制 器 的 设计 来 说 还 是 过 于 复杂 ， 因 此 需要 进一步 简化 。 
图 2. 10 为 根据 实验 数据 所 绘制 的 不 同 载荷 下 轮胎 纵向 力 及 侧 向 力 曲 线 。 从 
[4 2. 10 中 可 以 看 出 ， 在 侧 偏 角 及 纵向 滑 移 率 较 小 时 ， 轮 胎 力 可 以 用 线性 函数 
近似 描述 。 这 在 侧 向 加 速度 a,<0.4 g 的 情况 下 对 常规 轮胎 有 具有 较 高 的 拟 合 精 
度 在 这 个 范围 内 ， 用 以 下 算式 得 到 轮胎 的 纵向 力 和 侧 向 力 ; 

F, = Gs FE, = Gæ (2.34) 
式 中 、6, 为 轮胎 纵向 刚度 ，C, 为 轮胎 侧 偏 刚度 
在 之 前 所 建立 的 非 线性 模型 中 ， 存 在 较 多 的 三 角 琐 数 ， 对 于 模型 简化 有 较 
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0 
0.0 0.2 0.4 0.6 0.8 1.0 


滑 移 率 
(b) 


[82.10 轮胎 纵向 力 及 侧 向 力 曲 线 
(a) 侧 向 力 随 侧 偏 角 变化 曲线 ; (Cb) 纵向 力 随 滑 移 率 变化 曲线 
大 的 困难 。 因 此 ， 在 轮胎 力 的 计算 过 程 中 ， 提 出 小 角速度 假设 ， 即 满足 如 下 近 
似 条 件 : 


cos0O = 1, sind=Q, tang =Q 


式 中 9 表示 各 个 角 ， 包 括 前 轮 偏 角 ， 前 、 后 轮胎 侧 偏 角 等 。 
通过 以 上 简化 并 结合 式 (2. 19) ~ 式 (2.23)， 可 以 得 到 轮胎 侧 偏 角 的 计 
算 关 系 式 : 
LE VE ra (2.35) 
x X 
根据 式 (2.35) 和 线性 轮胎 模型 ， 就 可 以 很 容易 计算 得 到 轮胎 力 
前 、 后 轮胎 的 侧 向 力 : 


・ " . l & E 
Fu z c. [5 E L1 KE. - DT e- - ( 2. 36 ) 
x X 
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前 、 后 轮胎 的 纵 回 力 : 
Fir = G Fir = Crs, (2.37) 
将 以 上 简化 后 的 结果 代入 式 (2.29). 后 ， 得 到 了 基于 前 轮 偏 角 较 小 和 线 
性 轮胎 模型 假设 后 的 车 辆 动力 学 非 线性 模型 : 
my =- mxo + 2| cafa, - Ea t e) +C, ig i 一 "| 


X X 


mx = myg + 2| Cus 十 ca - Lt L8, 十 Gis, | 
x 
(2.38) 


am 
6: 
Hl 


2[ac.(s; - 一 - bC, 77 


X x 


-—. 
Nl 


Tsing + ycosw 


X = xcose - ysing 
在 该 系统 中 ,状态 量 选 取 为 &, iy. x. o. e. Y. . X]「, 控制 量 选取 为 
Uus =0, 
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在 初学 模型 预测 控制 时 往往 会 感觉 比较 抽象 。 模 型 预测 控制 作为 一 种 控制 
方法 ， 它 的 简单 在 于 思想 非常 贴近 我 们 的 日 常生 活 。 通 过 一 个 小 小 的 例子 就 能 
明白 它 的 核心 内 容 ， 但 要 深入 研究 模型 预测 控制 方法 ， 则 存在 大 量 py 
问题 和 控制 理论 问题 ， 在 数学 计算 复杂 性 、 控 制 实 时 性 和 稳定 性 问题 等 方 
还 有 大 量 的 问题 需要 我 们 去 解决 和 探索 。 

本 章 首 先 介绍 模型 预测 控制 基础 理论 ， 人 然后 扩展 到 两 类 常用 的 模型 预测 控 
制 方法 。 为 了 避免 在 开始 学 习 时 就 陷入 深奥 的 理论 中 ， 在 介绍 理论 时 ， 首 先 ， 
从 日 常生 活 中 的 一 个 案例 引出 模型 预测 控制 的 基本 思想 ; 其 次 ， 通 过 一 个 工程 
中 的 实例 进一步 加 深 对 这 种 方法 的 认识 ; 再 次 ， 分别 对 线性 模型 预测 控制 和 非 
线性 模型 预测 控制 进行 介绍 ， 从 代码 分 析 的 角度 出 发 ， 让 读者 能 够 尽快 编写 自 
己 的 控制 算法 。 


3.1 基本 理论 


3.1.1 生活 中 的 启示 


情景 如 下 : 团队 接 到 一 项 任务 ， 要 求 用 8 h 完成 。 根 据 我 们 已 有 的 经 验 ， 
我 们 知道 这 项 任务 可 以 分 解 成 8 个 子 任 务 ， 并 且 1 h 大 致 能 够 完成 一 项 ( 注 
意 : 并 不 是 一 定 可 以 完成 ) 。 那 么 ， 我 们 应 该 如 何 规划 这 8 h 呢 ? 也 许 很 多 人 
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o 
会 第 一 时 间 想 到 图 3. 1 中 所 示 的 方 案 : 既然 一 个 小 时 可 以 完成 一 项 任务 ， 那 肯 
定 就 是 每 个 小 时 完成 一 项 子 任务 ，8 h 结束 后 自然 就 可 以 完成 所 有 的 任务 了 。 


这 个 方案 虽然 具备 一 定 的 可 行 性 ， 但 具体 执行 时 还 是 会 遇 到 很 多 问题 ， 比 如 团 
队 成 员 临 时 离开 ， 某 一 个 子 任务 没有 按时 完成 ， 等 等 。 因 此 ， 这 个 方案 在 实际 
执行 过 程 中 过 于 理想 化 。 


9: 00 11: 00 13: 00 15: 00 17: 00 


子 任务 1 | 子 任务 2 | 子 任务 3 | 子 任务 4 | 子 任务 3 | 子 任务 6 | 子 任务 7 | 子 任务 8 





10: 00 12: 00 14: 00 16: 00 
图 3.1 工作 规划 方案 1 


为 了 解决 这 些 突 发 性 问题 ， 提 高 整个 工作 过 程 的 可 行 性 ， 经 过 思考 ， 我 们 
可 以 提出 第 2 种 方案 ， 也 就 是 图 3. 2 所 示 的 方案 。 在 这 个 方案 中 ， 我 们 不 再 规 
划 所 有 任务 ， 而 是 规划 出 未 来 一 段 时 间 (比如 4 h) 内 ， 依 然 是 每 个 小 时 完成 
其 中 一 项 任务 。 当 然 ， 在 这 一 段 时 间 内 ,依然 存在 发 生 突 发 情况 的 可 能 。1 h 
过 去 后 ， 还 需要 对 已 经 完成 的 任务 进行 总 结 。 在 新 的 h 开始 时 ， 首 先 需要 根 
据 过 去 完成 的 结果 重新 对 未 来 4 h 进行 任务 规划 。 如 果 上 一 个 小 时 没有 完成 规 
定 的 任务 ， 就 会 出 现 如 图 3. 3 所 示 的 规划 方案 。 在 下 一 个 时 刻 ， 就 需要 既 完 成 
上 一 时 刻 遗 留 下 来 的 任务 ， 也 需要 完成 本 时 刻 的 任务 。 根 据 这 样 的 规律 ， 我 们 
依次 在 新 的 时 刻 规 划 未 来 4 h 的 工作 计划 ， 并 及 时 更 正 这 样 的 计划 ， 最 终 完 成 
任务 


9: 00 11: 00 13: 00 


子 任务 1 | 子 任务 2 | 子 任务 3 | 子 任务 4 





10: 00 12: 00 


图 3.2 工作 规划 方案 2 












11: 00 13: 00 
F | 子 | 子 | 子 | 子 | 子 | 了 予 | 子 
任 | t€ | ££ | ££ | t£ EE E 
务 1 | 务 2 | 务 2 | 务 3 | 务 3 | 554 | 554 | 515 





10: 00 12: 00 


图 3.3 下 一 时 刻 新 的 规划 方案 
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显然 ， 方案 2 在 实际 工作 中 更 加 具备 可 操作 性 。 它 能 根据 任务 完成 情况 及 
时 调整 工作 计划 ， 而 较 短 的 规划 周期 也 使 得 出 现 突 发 情况 的 概率 更 小 。 在 完成 
任务 的 过 程 中 ， 我 们 可 以 总 结 出 以 下 3 个 关键 步骤 : 

CD 根据 已 有 的 知识 和 经 验 可 以 预计 任务 的 完成 情况 。 如 在 上 面 的 例子 中 ， 
团队 以 往 完成 任务 的 能 力 就 是 已 有 的 经 验 模 型 。 

(2) 只 规划 未 来 固定 一 段 时 间 的 任务 计划 ， 而 不 是 整个 工作 周期 。 

(3) 每 一 固定 时 间 段 结束 后 检查 任务 完成 情况 ， 然 后 根据 现 有 状态 重新 规 
划 下 一 段 时 间 的 任务 计划 。 

实际 上 ， 模 型 预测 控制 的 基本 思想 就 蕴含 在 这 3 个 步骤 中 。 它 利用 一 个 已 
有 的 模型 、 系 统 当 前 的 状态 和 未 来 的 控制 量 去 预测 系统 未 来 的 输出 〈 步 骤 1 ) 。 
这 个 输出 的 长 度 是 控制 周期 的 整数 倍 (步骤 2) 。 由 于 未 来 的 控制 量 是 未 知 的 ， 
还 需要 根据 一 定 的 优化 条 件 进行 优化 求解 ， 以 得 到 未 来 的 控制 量 序列 。 在 每 一 
个 控制 周期 结束 后 ， 系 统 根据 当前 实际 状态 重新 预测 系统 未 来 输出 〈 步 骤 3 ) 
关于 这 3 个 关键 步骤 的 进一步 介绍 将 在 下 一 节 展 开 。 


3.1.2 控制 理论 中 的 描述 


根据 上 一 节 ， 模 型 预测 控制 在 实现 过 程 中 有 3 个 关键 步骤 ,一般 被 称 为 3 
项 基本 原理 ， 分 别 是 预测 模型 、 滚 动 优 化 和 反馈 校正 ” 。 以 下 分 别 对 这 3 项 
基本 原理 进行 介绍 。 

① 预测 模型 : 预测 模型 是 模型 预测 控制 的 基础 。 其 主要 功能 是 根据 对 象 
的 历史 信息 和 未 来 输入 ， 预 测 系 统 未 来 的 输出 。 对 预测 模型 的 形式 没有 做 严格 
的 限定 ， 状 态 方程 、 传 递 函 数 这 类 传统 的 模型 都 可 以 作为 预测 模型 。 对 于 线性 
稳定 系统 ， 阶 路 响应、 脉冲 响应 这 类 非 参 数 模型 ， 也 可 以 直接 作为 预测 模型 
使 用 。 

D 滚动 优化 : 模型 预测 控制 通过 某 一 性 能 指标 的 最 优 来 确定 控制 作用 ， 
但 优化 不 是 一 次 离线 进行 ， 而 是 反复 在 线 进行 的 。 这 就 是 深 动 优化 的 含义 ， 也 
是 模型 预测 控制 区 别 于 传统 最 优 控制 的 根本 点 。 

(3) 反馈 校正 : 为 了 防止 模型 失 配 或 者 环境 干扰 引起 控制 对 理想 状态 的 偏 
离 ， 在 新 的 采样 时 刻 ， 首 先 检测 对 象 的 实际 输出 ， 并 利用 这 一 实时 信息 对 基于 
模型 的 预测 结果 进行 修正 ， 然 后 再 进行 新 的 优化 。 

基于 这 3 个 要 素 ， 模 型 预测 控制 的 基本 原理 可 以 用 图 3.4 来 表示 。 控 制 过 
程 中 ， 始 终 存在 一 条 期 望 参考 轨迹 ， 如 图 3.4 中 曲线 1 所 示 。 以 时 刻 大 作为 当 
前 时 刻 〈 坐 标 系 纵 轴 所 在 位 置 ) ， 控 制 器 结合 当前 的 测量 值 和 预测 模型 ， 预 测 
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系统 未 来 一 段 时 域内 [k, kN] (也 被 称 为 预测 时 域 ) 系统 的 输出 ， 如 
图 3.4 中 曲线 2 所 示 。 通 过 求解 满足 目标 函数 以 及 各 种 约束 的 优化 问题 ， 得 到 
在 控制 时 域 | た 、 大 +N.] 内 一 系列 的 控制 序列 ， 如 图 中 的 和 矩形 波 4 所 示 (从 
坐标 系 纵 轴 开 始 ) ， 并 将 该 控制 序列 的 第 一 个 元 素 作 为 受 控 对 象 的 实际 控制 量 。 
当 来 到 下 一 條 時 刻 ょ +1 时 ,重复 上 述 过 程 ， 如 此 滚动 地 完成 一 个 个 带 约束 的 
优化 问题 ， 以 实现 对 被 控 对 象 的 持续 控制 。 











过 去 将 来 

l aec a poe ponere 

i ーーー- 参考 轨迹 -1 
Kor aM TE^ 4 一 ~ -预测 输出 -2 

E | w- — —39 Pd ーー 测量 输出 -3 
im (T T =- 预测 控制 量 -4 


ーーー* 已 执行 控制 量 -5 


图 3.4 模型 预测 控制 原理 示意 


模型 预测 控制 原理 框图 如 图 3.5 所 示 , 包含 了 MPC 控制 器 、 被 控 平 台 
和 状态 估计 器 这 3 个 模块 。 其 中 ，MPC 控制 器 结合 预测 模型 、 目 本 函数 和 釣 
束 条 件 进 行 最 优化 求解 ， 得 到 当前 时 刻 的 最 优 控制 序列 u^ (4) ， 输 入 到 被 控 
平台 ,被 控 平台 按照 当前 的 控制 量 进行 控制 ， 然 后 将 当前 的 状态 量 观测 值 
xf(1) 输 入 给 状态 估计 器 。 状 态 估 计 器 对 于 那些 无 法 通过 传感器 观测 得 到 或 者 
观测 成 本 过 高 的 状态 量 进 行 估 计 。 比 较 常 用 的 方法 有 Kalman 滤波 、 粒 子 滤 
波 等 。 将 估计 的 状态 量 x*(1) 输入 到 MPC 控制 器 ， 再 次 进行 最 优化 求解 ， 以 
得 到 未 来 一 段 时 间 的 控制 序列 。 如 此 循环 ， 就 构成 了 完整 的 模型 预测 控制 
过 程 

根据 所 采用 模型 的 不 同 ， 模 型 预测 控制 主要 包括 动态 惩 阵 控制 (Dynamic 
Matrix Control, DMC) 、 模 型 算法 控 制 (Model Algorithm Control, MAC), J^ X 
预测 控制 (Generalized Predictive Control, GPC) 等 。 同时， 在 现代 控制 理论 中 
广泛 使 用 的 状态 空间 模型 ， 同 样 可 以 应 用 于 模型 预测 控制 中 。 本 书 立 足 于 模型 
质 测 控制 在 无 人 玖 驶 车 辆 方向 的 应 用 ， 因 此 重点 放 在 基于 状态 空间 模型 的 模型 
mda E 
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图 3.5 MPC 控制 原理 框图 


3.2 一 个 简单 的 实例 


通过 第 3. 1 节 中 的 引 例 和 理论 介绍 ， 对 模型 预测 控制 的 3 项 基本 原理 有 了 
一 定 的 认识 。 下 面 通过 在 Matlab 中 的 一 个 简单 实例 ， 分 析 模 型 预测 控制 在 实 
际 应 用 时 的 基本 思路 。 

Matlab 中 的 模型 预测 控制 工具 箱 ( Model Predictive Control Toolbox) 是 集 
成 在 Matlab 环境 下 的 工具 包 之 一 ， 提 供 了 一 系列 用 于 模型 预测 控制 的 分 析 、 
设计 和 仿真 的 函数 。 这 个 工具 箱 使 用 简单 ， 能 够 帮助 初学 者 快速 了 解 MPC 的 
基本 概念 和 应 用 方法 ; 同时 ， 工 具 箱 还 提供 了 一 系列 实际 控制 例 程 作为 教学 使 
用 。 我 们 在 开始 学 习 模 型 预测 控制 时 ， 就 可 以 从 这 些 案例 出 发 。 

首先 需要 打开 Matlab 的 例 程 教学 窗口 。 在 Windows 中 运行 Matlab 主 程序 ， 
并 在 控制 命令 窗口 输入 “demo” 命 令 ， 出 现 如 图 3.6 所 示 界 面 。 在 左 侧 目录 
中 可 以 看 到 众多 工具 箱 ， 如 神经 网 络 工具 箱 、 模 糊 工 具 箱 等 。 找 到 Model Pre- 
dictive Control Toolbox, ， 并 将 该 项 目 展 开 ， 可 以 看 到 一 系列 子 项 目 ， 其 中 Demos 
就 是 需要 用 到 的 例 程 。 点 击 Demos 下 的 Tutorial 并 选择 第 一 个 项 目 “MPC Con- 
trol of Double Integrator”， 可 在 右 侧 窗 口 看 到 这 个 例 程 的 介绍 、 源 代码 以 及 结果 
展示 。 

为 了 直观 地 了 解 这 个 例 程 的 功能 ， 可 以 运行 它 查 看 结果 。 一 般 有 两 种 方 
法 : 一 种 就 是 点 击 右 上 角 的 “Run in the Command Window”， 如 图 3. 7 p% 
色 和 矩形 框 所 示 。 进 入 Matlab 控制 界面 后 按照 提示 就 可 以 看 到 每 一 步 的 运行 
过 程 。 

另外 一 种 方法 是 通过 运行 相应 的 mm 文件， 点 击 主 窗口 左上 角 的 “Open 
mpcdoubleint. m in the Editor”， 如 图 3.8 中 黑色 和 矩形 框 所 示 。 然 后 程序 会 自 
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matlab:edit mpcdoubleint 


图 3.8 运行 例 程 方法 2 
动 打 开 相 应 的 m 文件 。 点 击 运 行 按 钮 或 者 键盘 片 ， 都 可 以 直接 运行 这 个 程序 。 
经 过 一 段 时 间 的 计算 后 ， 程 序 会 显示 运行 结果 并 且 自 动 关闭 窗口 。 如 果 要 仔细 
观察 控制 结果 ， 可 以 将 m 文件 的 最 后 两 行程 序 注释 掉 ， 即 : 





る bdclose('mpc doubleint ^); 
$ displayEndOfDemoMessage (mfilename) 





运行 后 结果 如 图 3.9 所 示 。 它 包含 3 个 窗口 ， 左 侧 窗口 为 Simulink 环境 下 
搭建 的 控制 系统 ， 右 侧 两 个 窗口 分 别 为 被 控 系统 的 输入 和 输出 。 从 被 控 系 统 输 
出 我 们 可 以 看 出 ， 该 系统 的 控制 目标 是 跟踪 一 条 固定 轨迹 。 系 统 状态 从 0 出 
发 ， 在 控制 器 的 作用 下 ， 逐 渐 跟踪 上 了 期 望 轨迹 。 从 被 控 系 统 输入 可 以 看 出 ， 
输入 值 被 成 功 限 定 在 [ -1，1] 区 间 。 下 面 介 绍 仿真 实例 中 MPC 控制 系统 如 
何 实现 模型 预测 控制 功能 。 

首先 ， 对 整个 控制 系统 进行 分 析 。 除 去 输入 、 输 出 显示 ， 整 个 控制 器 主要 
由 两 部 分 内 容 组 成 : 模型 预测 控制 器 和 被 控 系统 ( 双 积 分 系统 )， 如 图 3.10 


5rjs.cn 000000 


o 第 3 章 ”模型 预测 控制 算法 基础 与 仿真 分 析 


Bras. 具体 的 输入 、 输 出 关系 如 下 : 模型 预测 控制 器 的 输入 为 参考 轨迹 和 系统 
状态 ， 而 被 控 对 象 的 输入 则 为 控制 器 输出 的 控制 量 ， 被 控 对 象 的 输出 即 状态 
量 ， 青 反馈 输入 到 MPC 控制 器 中 。 明 确 了 输入 、 和 输出 关系 后 能 够 为 后 续 自 行 
设计 控制 系统 葛 定 基础 . 
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图 3.9 运行 结果 










> | 
integrator). in tegrator2, 







Constant 


Tidi ds 








To Workspacel 





Outputs/References 








图 3.10 控制 系统 分 析 


在 对 整个 控制 系统 有 了 基本 概念 后 ， 再 来 了 解 算法 程序 代 但 ， 探 究 一 个 
标准 的 模型 预测 控制 器 究竟 如 何 工作 ， 在 之 前 打开 的 “mpedoubleint m” X 
件 中 ， 首 先是 对 程序 的 说 明 以 及 版 权 申 明 ， 然 后 按照 不 同 的 步 骂 分 块 进行 程 
序 编 写 。 今 后 我 们 在 编写 自己 的 控制 算法 时 ， 也 可 以 参考 这 样 的 写法 。 在 程 


序 主体 内 容 中 ， 我 们 将 重点 放 在 模型 陆 测 控制 器 的 设置 上 面 ， 也 就 是 如 下 一 
段 代 但 : 
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oO 
TBS ュ ユ > る Sampling time 
p220; $ Prediction horizon 
mz3; $ Control horizon 


mpc. controller -mpc (t£ (1,[1 0 0]), Ts,p,m); 
$ MPC object 
mpc. controller.MV -struct('/Min'/,-1,'Max'/,1); 
$ Input saturation constraints 

这 段 代码 完成 了 构建 模型 预测 控制 器 必要 的 设置 。 首 先是 对 系统 采样 时 间 
的 设置 ; 其 次 是 确定 了 预测 时 域 和 控制 时 域 (这 两 者 的 区 别 将 在 之 后 的 章节 中 
介绍 ); 再 次 是 通过 调用 系统 函数 ff( ) 以 传递 函数 的 形式 实现 系统 建 模 ; 最 后 
通过 调用 模型 预测 工具 箱 函 数 mpc( ) 完成 控制 器 的 设 定 。 为 了 实现 对 控制 输 
人 的 限制 ， 代 码 还 加 入 了 控制 量 的 上 下 界 约束 条 件 。 

一 般 来 说 ， 模 型 预测 控制 器 需要 给 定 被 控 系 统 的 采样 时 间 、 预 测 时 域 、 控 
制 时 域 和 预测 模型 。 如 果 需 要 加 入 系统 的 约束 ， 还 需要 进一步 给 定 约束 条 件 . 
不 仅仅 是 在 Matlab 自 带 的 工具 箱 中 ， 其 他 版 本 的 工具 箱 (如 MPC tools) 也 是 
在 此 基础 上 扩展 的 。 读 者 在 编写 控制 算法 的 时 候 ， 也 建议 将 这 些 参数 作为 基本 
的 传递 参数 。 这 样 不 仅 能 保证 设置 准确 可 靠 ， 还 能 提高 代码 的 可 读 性 。 

由 于 是 在 Simulink 环境 下 的 仿真 ， 故 还 需要 对 仿真 环境 进行 设置 ， 包 括 系 
统 初始 状态 、 仿 真 时 间 和 参考 轨迹 ， 代 码 如 下 : 


x01lsz0; $ Initial state:First integrator 

x02 20; $ Initial state:Second integrator 

Tstop-5; $ Simulation time 

Tu. $ Set point 

最 后 ， 打 开 搭 建 的 Simulink 模型 ， 运 行 仿真 程序 ， 就 完成 了 整个 控制 
系统 。 


分 析 了 整个 程序 之 后 ， 会 发 现 这 个 例子 其 实 非 常 简单 。 通 过 简单 的 参数 设 
置 和 环境 设置 ， 就 可 以 完成 一 个 双 积分 系统 对 固定 轨迹 的 跟踪 ， 并 且 对 输入 、 
输出 进行 了 约束 设 定 。 为 了 加 深 对 程序 的 理解 ， 我 们 可 以 自己 尝试 修改 相关 会 
数 ， 如 预测 时 域 、 采 样 时 间 等 ， 观 看 不 同 参数 的 控制 效果 。 在 对 模型 预测 控制 
有 了 一 定 的 认识 后 ， 可 以 分 别 修改 控制 器 与 被 控 对 象 ， 搭 建 自己 的 仿真 平台 ， 
通过 模型 预测 控制 工具 箱 中 的 例 程 ， 对 模型 预测 控制 如 何 使 用 已 经 有 了 直 
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观 的 认识 ; 但 在 刚才 的 分 析 中 ， 只 了 解 了 如 何 去 设 置 模型 预测 控制 器 的 参数 ， 
但 并 没有 涉及 模型 预测 控制 器 的 内 部 结构 。 打 开工 具 箱 函 数 mpe( ) ff m x 
件 ， 发 现 程序 比较 复杂 ， 一 时 间 难 以 看 懂 。 这 时 候 就 需要 我 们 掌握 将 模型 预测 
控制 问题 转换 为 计算 机 算法 程序 应 用 的 方法 ， 同 时 还 要 补充 一 些 控制 系统 的 知 
识 。 只 有 建立 在 良好 的 理论 基础 之 上 ， 才 能 开始 编写 核心 的 控制 算法 。 


3.3 ”线性 时 变 模型 预测 控制 算法 


3.3.1 问题 描述 


线性 时 变 模型 预测 控制 算法 是 以 线性 时 变 模型 作为 预测 模型 的 。 这 也 是 目 
前 在 模型 预测 控制 领域 中 应 用 最 为 广泛 的 一 种 形式 。 相 比 于 非 线性 模型 预测 控 
制 ， 它 的 最 大 优点 是 计算 较为 简单 ， 实 时 性 好 。 对 于 无 人 驾驶 车 辆 的 运动 控制 
来 说 ， 控 制 算法 的 实时 性 显然 是 至 关 重 要 的 ， 因 此 线性 时 变 模型 预测 控制 是 需 
要 重点 介绍 的 一 种 MPC 控制 方法 。 

在 之 前 的 介绍 中 ,已 经 反复 强调 了 模型 预测 控制 器 的 3 項 基本 原理 。 以 

， 以 线性 状态 空间 模型 为 基础 ， 依 次 推导 模型 预测 控制 的 预测 方程 、 优 化 求 

馈 机 制 

(1) 预测 方程 

首先 ， 考 虑 以 下 的 离散 线性 化 模型 : 





x(k 1) =A, x(k) * B,,u(k) (3.1) 
设 定 
x(klt) 
6 - 3.2 
Rutas s an: 





可 以 得 到 一 个 新 的 状态 空间 表达 式 : 
£(k 111) = A, を (を 1 0) B, Au(KI t) 
nikl t) = Cu を (を 1 の) 

式 中 各 征 降 的 定 文 如 下 : 


A, = | tu B, Jos e si (3.4) 
Jos I, 


C= CC, 8] 
为 了 进一步 简化 计算 ， 做 出 如 Ki 设 : 


(3.3) 
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ssd, bsl,-uu4N-1 

e=, た =1…。/+ が パー1 
如 果 系 统 的 预测 时 域 为 N,， 控 制 时 域 为 NM., 那么 ， 预 测 时 域内 的 状态 量 

和 系统 输出 量 可 用 以 下 算式 计算 : 

E+N, Lt) = ÅPEN t) + A? B, Au(tl t) + + A B Nut NL t) 


A 
" (3.5) 
B 


(3.6) 
qt +N, l t) =C, AMEC t) CAI B,Au(tl p + + 
CAP B Ault N, IE t) (3.7) 
为 了 使 整个 关系 更 加 明确 ， 将 系统 未 来 时 刻 的 输出 以 矩阵 的 形式 表达 : 
Y(t) = W£(tlt) +0,AU(C) (3.8) 
式 中 : 
gt ell) CA, 
7(/+21 の ) CA; Au(tl t) 
… Au(t 11 t) 
Y = = U = 
| | | TU n 
- Au(t - N. I1) 
nt +N, | £) CA^ 
C B, 0 0 
C,A,B, ČB, 0 
O, = CA~ 'B, CA; B, C,B, 
CA*B, CA"B, CAB 


CAN B, CAY^B, 


F toa 


通过 观察 式 (3.8) ， 可 以 清楚 地 看 到 ， 在 预测 时 域内 的 状态 量 和 输出 量 
都 可 以 通过 系统 当前 的 状态 量 #(11 1) 和 控制 时 域内 的 控制 增 量 AU(1) 计算 
得 到 。 这 也 就 是 模型 预测 控制 算法 中 “预测 ”功能 的 实现 

(2) 优化 求解 

实际 上 ， 系 统 的 控制 增 量 是 未 知 的 ， 只 有 通过 设 定 合适 的 优化 日 标 ， 并 对 


其 进行 求解 ， 才 能 得 到 控制 时 域内 的 控制 序列 ， 


的 目标 函数 : 
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JG) = DX G jl k) (k +j) «a'(k ej -1)Ra(k *j - 1) 
j=l 


(3.9) 

对 于 以 上 形式 的 优化 目标 ， 可 以 通过 适当 的 处 理 将 其 转换 为 二 次 规划 
( Quadratic Programming, QP) 问题 。 二 次 规划 是 一 个 则 型 的 数学 优化 问题 。 
它 的 优化 目标 是 二 次 实 函 数 ， 带 有 线性 或 者 非 线 性 约束 。 其 常用 的 解法 为 有 效 
集 法 (Active Set Method, ASM) MAHIA (Interior Point Method，IPM)。 有 效 
集 法 适用 于 解决 只 有 不 等 式 约束 的 二 次 规划 问题 ， 而 内 点 法 适用 于 任何 形式 的 
二 次 规划 问题 . 文献 [50] 已 详细 介绍 了 将 模型 预测 控制 问题 转换 为 二 次 规 
划 问 题 的 方法 ， 本 章 第 3.5 节 给 出 了 基于 线性 二 次 型 最 优 控制 的 实例 ， 并 给 出 
了 算法 代码 . 

x (3.9) 中 以 控制 量 作 为 目标 函数 中 的 状态 量 ， 结 构 简 单 ， 易 于 实现 ; 
但 也 存在 一 些 缺 点 ， 比 较 显著 的 就 是 没 法 对 控制 增 量 进行 精确 约束 。 当 系统 对 
于 控制 量 跳 变 要 求 较为 严格 时 ， 这 样 的 目标 也 数 就 无 能 为 力 了 。 此 时 ， 可 以 把 
控制 增 量 作为 日 标 函 数 的 状态 量 ， 优 化 目标 也 数 可 以 设 为 如 下 形式 : 


KE) ult 1) AO(7) ) = Y, [ne til t) -Malt illo 
(zl 


N= 


Y, || Aul +il D |i (3. 10) 


T 
其 中 ， 第 一 项 反映 了 系统 对 参考 轨 线 的 跟随 能 力 ， 第 二 项 反映 了 对 控制 量 平稳 
变化 的 要 求 。@ HIR 为 权重 和 矩阵， 整个 表达 式 的 功能 是 使 系统 能 够 尽快 且 平 
稳 地 跟踪 上 期 望 的 轨迹 。 同 时 ， 在 实际 的 控制 系统 中 ， 往 往 需 要 满足 系统 状态 
量 以 及 控制 量 的 一 些 约束 条 件 ， 一 般 如 下 : 
控 制 量 釣 東 : 
nll tk) &u(rck) su, (tk), kz0,1,--,N, -1 (3.11) 
控制 增 量 约束 : 
Au „(t +k) < Ault +k) < Au (Ck), kz20,1,--, NN, -1 


(3. 12) 


Tas 


输出 约束 : 
Kt iI tk k=O er -1 (3.13) 
式 (3.10) -(3.13) 就 形成 了 一 个 完整 的 优化 日 慰 表 达 式 。 通 过 求解 这 
个 带 约 东 条 件 的 优化 日 标 ， 就 能 得 到 未 来 一 段 时 间 的 控制 序列 。 然 而 ， 由 于 系 
统 的 模型 是 实时 改变 的 ， 并 不 能 保证 每 个 时 刻 该 优化 日 标 邦 能 得 到 可 行 解 。 因 
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此 ， 有 必要 对 优化 目标 进行 相应 的 处 理 。 比 较 普 遍 并 且 证 明 有 效 的 方式 是 在 优 
化 目标 中 加 入 松弛 因子 ， 如 式 (3.14) 所 示 : 

7( を (/) ,u(t -1),AU(t)) = Y mt +ilt) «au +il1) lo 十 

Y | Au(t il t) |} * pe (3.14) 

式 中 , o 为 权重 系数 ，s 为 松弛 因子 。 

将 式 (3.8) 代入 优化 目标 式 (3.14) ， 并 且 将 预测 时 域内 的 输出 量 偏差 
表示 为 : 

Elt) = W£( t) = Yt) Yar = [halt tL 8 mat FN, LD] 


(3.15) 
经 过 相应 的 矩阵 计算 ， 可 以 将 优化 目标 调整 为 : 
JCECO ,u(t -1),AU(t)) S [AUGCO  ,e)' HLLAUGO ,se] + 
G,[AU(CD) ,es] +P, (3.16) 
T 
R l 
式 中 : H, = oir d x , G, -[2bE(0)'9Q.0, 0], P, -E()'Q.E(O. 


p 
在 式 (3.16) F, P, 为 常量 ， 因 此 模型 预测 控制 在 每 一 步 的 带 约束 优化 
求解 问题 都 等 价 于 求解 如 下 的 二 次 规划 问题 : 
OPEM M M + G, [AU(t)", e] 


st AU,, < AU(k) = , k=t,…,t+H.-!] 
Oa <ul -1) + X AUG) < k = t,t +H, -1 


Y,-esWZ(tlt) +OAUCG) S Yn tE 
e>0 
(3) 反馈 机 制 
在 每 个 控制 周期 内 完成 对 式 (3. 17) 的 求解 后 ， 得 到 了 控制 时 域内 的 一 
系列 控制 输入 增 量 : 
AU; = [Au/ , Au, … AL (3.18) 
根据 模型 预测 控制 的 基本 原理 ， 将 该 控制 序列 中 第 一 个 元 素 作 为 实际 的 控 
制 输 入 增 量 作用 于 系统 ， 即 : 
u(t) = (7 一 1) + Au, (3. 19) 
系统 执行 这 一 控制 量 直 到 下 一 时 刻 。 在 新 的 时 刻 ， 系 统 根据 状态 信息 重新 


5rjs.cn 000000 


- 第 3 章 ”模型 预测 控制 算法 基础 与 仿真 分 析 





预测 下 一 段 时 域 的 输出 ， 通 过 优化 过 程 得 到 一 个 新 的 控制 增 量 序列 。 如 此 循环 
往复 ， 直 至 系统 完成 控制 过 程 。 


3.3.2. 非 线性 系统 线性 化 方法 


根据 第 2 章 车 辆 系统 建 模 的 知识 ， 我 们 知道 ， 无 论 是 运动 学 建 模 ， 还 是 动 
力学 建 模 ， 得 到 的 都 是 一 个 非 线 性 系统 。 显 然 ， 这 种 系统 是 不 能 直接 用 于 线性 
时 变 模 型 预测 控制 的 。 本 节 介 绍 如 何 将 一 般 的 非 线 性 系统 进行 线性 化 处 理 ， 以 
得 到 需要 的 线性 时 变 系统 

将 一 个 非 线 性 系统 近似 为 线性 时 变 系 统 有 很 多 方法 ， 大 体 可 以 分 为 近似 线 
性 化 与 精确 线性 化 两 类 。 近似 线 性 化 方法 简单 ， 适 用 性 较 强 ， 缺 点 是 在 控制 精 
度 要 求 非常 高 的 场合 难以 适用 ; 精确 线性 化 一 般 不 具备 普遍 性 ， 往 往 是 需要 针 
对 单个 系统 进行 县 体 分 析 。 在 模型 预测 控制 中 ， 一 般 采 用 近似 的 线性 化 方法 。 
以 下 介绍 两 种 常用 的 线性 化 方法 

方式 A: 存在 参考 系统 的 线性 化 方法 

该 方法 的 主要 思想 是 假设 参考 系统 已 经 在 期 望 路 径 上 完全 通过 ， 得 到 了 路 
径 上 每 个 时 刻 的 状态 量 和 控制 量 ” 。 通 过 对 参考 系统 和 当前 系统 之 间 的 偏差 
处 理 设计 使 模型 预测 控制 希 来 跟踪 期 望 路 径 。 参 考 系 统 的 任意 时 刻 的 状态 和 控 
制 量 满足 如 下 关系 : 

を - f(£,.u,) (3.20) 

在任 意 点 (を 、 ぁ を ) 处 进行 泰勒 展开 ， 只 保留 一 阶 项 ， 忽 略 高 阶 项， 
得 到 ; 








af af 
xi zy = r3 一 3. 21 
E = f(£,,u,) + a NL £.) ur NL u,) ( ) 
也 可 以 写 为 : 
=f nu) + J(£)(£ -£) * J/(u)(u -u,) (3.22) 


P, J, C£) 为 /相对 于 专 的 雅 可 比 矩 阵 ，J Cu). 为 /相对 于 的 雅 可 比 
矩阵 
将 式 (3.22) 与 式 (3.20) 相 减 可 以 得 到 : 
を = A(CO£ + BDA (3.23) 
式 中 , £-£-£. u-u-u,, A(t) =J (£), BG) =J (u). 
这 样 ， 就 得 到 新 的 状态 方程 。 该 状态 方程 是 连续 的 ， 不 能 直接 用 于 模型 预测 
控制 器 的 设计 ， 故 有 必要 对 其 进行 离散 化 处 理 。 此 处 采用 近似 的 离散 化 ， 即 ; 
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A,, 2 E 74(/) 


(3.24) 
B,, = TB(1) 
结合 式 (3.23) 和 式 (3.24), ， 可 以 得 到 : 
を (た +1) = A, Z(K) +B, a(k) (3.25) 


至 此 ， 得 到 了 非 线 性 系统 在 任意 一 个 参考 点 处 线性 化 后 的 系统 。 该 系统 是 
设计 线性 模型 预测 控制 算法 的 基础 。 

方式 B: 针对 状态 轨迹 的 线性 化 方法 

该 方法 的 主要 思想 是 通过 对 系统 施加 持续 不 变 的 控制 量 ， 得 到 一 条 状态 轨 
迹 ， 根 据 该 状态 轨迹 和 系统 实际 状态 量 的 偏差 设计 线性 模型 预测 控制 算法 。 这 
种 方法 主要 的 优势 是 不 需要 预先 得 到 期 望 跟 踊 路 径 的 状态 量 和 控制 量 。 如 果 方 
式 A 中 参考 系统 的 控制 量 始终 不 变 ， 则 与 此 方法 等 同 

考虑 系统 的 某 个 工作 点 为 [如 Uol, E(k) 为 始终 施加 控制 量 为 uo 后 得 
到 的 系统 状态 量 ， 则 存在 以 下 关系 : 

を (た +1) = f(& (E) ao) 
é0(0) = Éo 
经 过 和 方式 A 类 似 的 推导 ， 可 以 得 到 ; 
を (た +1) = 4 去 (下 ) +B, ū(k) 

式 中 , ま = ま (な ) -&(k), a -u(k) -u, 

针对 该 方法 还 有 其 他 的 数学 处 理 方法 ， 具 体 可 参考 文献 [70]; 同时 ， 除 
了 以 上 两 种 方法 外 ， 还 有 其 他 的 一 些 线性 化 方法 ,例如 分 段 线性 化 等 ， 可 参考 
文献 [71]. 


3.3.3 工程 实例 


上 一 节 我 们 从 理论 上 推导 了 线性 时 变 模型 预测 控制 ， 而 本 节 将 通过 一 个 工 
程 实例 进一步 加 强 读者 对 这 一 理论 的 认识 ， 并 给 出 相应 的 程序 代码 和 分 析 

(1) 控制 系统 简介 

以 无 人 驾驶 车 辆 的 轨迹 跟踪 问题 作为 应 用 背景 。 轨迹 跟 踪 是 机 器 人 运动 控 
制 的 基本 问题 之 一 。 无 人 驾驶 车 辆 在 一 个 给 定位 置 出 发 ， 通 过 离散 轨迹 点 或 者 
连续 轨迹 函数 的 指引 ， 最 终 跟 踪 上 期 望 轨迹 。 详 细 的 介绍 可 以 阅读 文献 
[50], 

轨迹 跟踪 仿真 基本 设 定 为 : 车 辆 从 坐标 原点 出 发 ， 以 期 望 纵向 速度 w=1 
m/s 跟 踩 一 条 直线 轨迹 y=2。 采 样 时 间 为 50 ms ,仿真 总 时 间 设 定 为 20 s, f) 
真 环境 为 Matlab。 
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(2) 问题 分 析 
由 于 控制 目标 是 无 人 驾驶 车 辆 在 低速 情况 下 的 跟踪 控制 ， 因 此 考虑 以 车 辆 
运动 学 方程 作为 预测 模型 。 根 据 第 2 章 的 介绍 ， 低 速 情况 下 的 车 辆 运动 学 方程 


形式 如 下 : 
k, COS の 
y, E | sing I 
e tan8,/1 

对 其 进行 线性 化 ， 得 到 线性 时 变 模型 为 ; 


&£,(k*1) = AL (ke (k) + Bi (Cb (3.26) 
式 中 ,各 和 矩阵 和 状态 变量 为 


天 一 区 1 0 -vsing,T 
PEL |， = 4 (な ) = 【 | pcosoe,7 | 
の ー の, 0 0 1 
cose, T 0 
BU) = の 
tanó, ,7 v,T 


l Lcos?’ (8,,) 





T 为 采样 时 间 

(3) 代码 解析 

在 Matlab 环境 下 对 无 人 驾驶 车 辆 的 直线 轨迹 跟踪 过 程 进行 仿真 。 为 了 更 
清晰 地 分 析 整 个 程序 ， 将 仿真 程序 chapter -3 -3. m 分 为 寿 干 模块 ， 分 别 介 
£28. 第 1 部 分 是 参考 轨迹 点 生成 的 程序 : 





を chapter3 -3-3.m $$ 1 部 分 
g 4 5 yl p. 


indes; * 参考 轨迹 点 数量 
TOWER * 采样 周期 


Xout = zeros (N,3); 
Tout - zeros (N,1); 
for kz-z1lslsN 
Xout (k,1)- k* T; 
Xout (K,2)22; 
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Xout (k,3)20; 
Tout (k,1 )= (k-1) * T; 
end 
参考 轨迹 点 生成 是 根据 仿真 设 定 给 出 的 。 读 者 可 以 尝试 自己 编写 其 他 形式 
的 参考 轨迹 ， 如 圆 形 轨迹 、8 字形 轨迹 等 。 
程序 chapter3 -3 -3. m 第 2 部 分 主要 描述 控制 系统 的 基本 情况 ,包括 状态 
量 和 控制 量 个 数 、 初 始 状 态 等 : 


$ chapter3 -3 -3.m 第 2 部 分 
& 仿真 系统 基本 情况 介绍 


Nx-3; gs 状态 量 个 数 
Nu =2 : を 控制 量 个 数 


[Nr ,Nc]= size (Xout); 
Tsim=20 ;仿真 时 间 
X0 =[0 0 pi /3]; 车 辆 初始 状态 


L=1; S 车 辆 轴 距 
vdl =1; & 参考 系统 的 纵向 速度 
vd2 =0; % 参考 系统 的 前 轮 偏 角 


程序 chpater3 -3 -3. m 第 3 部 分 是 根据 控制 系统 的 维度 信息 ， 提 前 定义 好 
相关 和 矩阵 并 赋值 。 考 虑 到 读者 大 多 数 都 是 第 一 次 编写 模型 预测 控制 的 应 用 程 
序 ， 此 处 以 文献 [70] 中 的 方法 为 基础 编写 。 对 于 介绍 的 第 2 种 目标 函数 及 
其 解法 ， 将 在 第 4 章 中 米 用 。 





$ chapter3 -3 -3.m 第 3 部 分 

g 矩阵 定义 

x_real =zeros (Nr ,NC ) : 

X piao-zeros (Nr ,NC) : 

u, real = zeros (Nr , Nu ) : 

u, piao = zeros (Nr ,NU ) : 

x realí(l,:)2X0; 

x piao(1,:)-x real(1,:)-Xout (1,:); 

X PIAO-zeros (Nr ,Nx * Tsim); 

XXX = zeros (Nr,Nx * Tsim);* 用 于 保存 每 个 时 刻 预 测 的 所 有 状态 值 
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q=[100;010;000.5]; 
Qcell=cell (Tsim,Tsim); 
for i=1 :I Isim 


for j =1:1 :Tsim 


if 122] 
Q cellíi,j)eq: 
else 
Q cellíi,j)-zeros (Nx,NXx); 
end 
end 
end 


Q-cell2mat (QO cell); 

R=0.1 * eye (Nu * Tsim,Nu * Tsim); 

程序 chapter3 -3 -3. m 第 4 部 分 是 模型 预测 控制 的 主体 。 在 每 一 控制 周期 
的 开始 ， 获 取 系 统 当 前 状态 量 ， 更 新 状态 空间 方程 。 根 据 3. 3. 1 节 中 的 方法 ， 
求解 一 个 标准 二 次 规划 问题 。 将 所 求 序 列 的 第 一 个 解 施 加 到 系统 上 。 巾 于 本 仿 
真 程序 只 运行 在 Matlab 环境 下 ， 因 此 车 辆 的 运动 过 程 以 运动 微分 方程 求解 的 
形式 产生 。 为 了 直观 体验 模型 预测 控制 在 车 辆 跟踪 过 程 中 的 控制 效果 ， 加 入 了 
绘图 程序 ， 这 样 能 够 动态 显示 车 辆 运行 结果 。 





$ chapter3 -3 -3.m 第 4 部 分 
for i =1 Nr 
t dzXout (i,3); 
a =[1 0 -vdl*sin(t d)*T; 
0 1 vdi * cos(t d) *T; 
0 0 liis 
be[cóstt d)x*'T Qs 
sin(t d)*T 0; 
vd2 * T/L vdl * T/(cos (vd2)^2);]; 
A cell-cell (Tsim,1 ) 
B cell-cell(Tsim,Tsim); 
For コー は LR 


A cellí(j,1)-a^j; 
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for ksi:1:Tsim 
if kj 
B cellí(j,k)-(a^ (j -k)) * b; 
else 
B. cell(j,k)- zeros (Nx,Nu); 
end 
end 
end 
A-cell2mat (A cell); 
B-cell2met (B cell); 


Hz2* (B'*Q*B-«R); 

f-2*B'*Q*A*x piao(i,:); 

A cons =[] : 

b. cons =[] 

lbs[-2.2;-0.64]; 

ub 2[0.2;0.64]; 

[X,fval(i,1),exitflag (i,1),output (1,1 ) ] = quadprog (H, f, 


A. cons,;b. cons, [],[],1b,ub); 


X_PTAO (1 , :)=(A*x piao(i,:) «B*X); 

if i+j<Nr 
for j=1:1:Tsim 
XXX (1,143 * (j-1))-X PIAO(,1 +3 *(] -1})}-Xout (i +j, Ls 
XXX (1,2 +3 » (j -1))=X_PIAO (i,2 +3 *(j -1))«Xout (14 j,2); 
XXX (1,3 +3 * (j -1))=X_PIAO (i,3 +3 *(j -1))+Xout (i +j,3); 
end 


else 


for j21:1:Tsim 

XXX (i,1 +3 * (j -1))=X_PIAO (i,1l +3 *(j-1))+Xout (Nr,1); 

XXX (1,2 +3 *(j -1))-X PIAO(i,2*3* (j-1))« Xout (Nr,2); 

XXX (1,3 +3 *(j -1))- X. PIAO (1,3 +3 *(j -1 ) )+ Xout (Nr,3); 
end 


end 


u piao(i,1)sX (1,1); 


5rjs.cn 000000 


第 3 章 ”模型 预测 控制 算法 基础 与 仿真 分 析 


u piao(i,2)-X 2,1); 
Tvec =[0 :0.05 :4]; 
X00 2x real(i,:); 
vdll =vdl +u_piao (i ,1 ); 
vd22 =vd2 +u_piao (1,2 ); 
XOUT = dsolve (/Dx- vd11 * cos (z)20 ^, "Dy - vdll * sin (z)20 ', 
-vd22 20, x(0)2 X00 (1) ; y (0) 2 X00 2) ; 'z(0) 2 X00 (3)) ; 
E= Fe 
x_real (i +1 ,1)=eval (XOUT. x); 
x_real (i +1,2)=eval (XOUT. y); 
x_real ( ュ +1 ,3 )= eva1 (XOUT. z ) : 
if (1 <Nr ) 
x piao(i-1,:)2x real(i-«1,:)-Xout (i-*1,:); 
end 
u real(i,1)2vd1 «u, piao(i,1); 


u real (i,2)-vd2 «u, piao(i,2); 


figure (1); 
plot (Xout (1 :Nr,1),Xout (1:Nr,2)); 
hold on; 
plot (x real (i,1),x real(i,2), r*^; 
xlabel (‘x [m] ) 
axis([-15 -13]); 
ylabe1 ( Y [m] 7): 
hold on; 
for ksi1:1:Teim 
X(i,k«1)-2 XXX (1,1 43 *(k-1)); 
Y (i,k«1)2 XXX (1,2 *3 *(k -1)); 
end 
X(i,l)2x real (i,1); 
Y (1,1)2 x real (1,2); 
plot iX lisk listh Y ) 
hold on; 
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end 

$ 程序 结束 

结合 以 上 4 个 部 分 ,在 Matlab 环境 下 运行 可 以 得 到 结果 ， 如 图 3. 11 所 
示 。 图 3. 11 中 的 曲线 a 为 期 望 轨迹 ， 星 号 曲线 b 为 车 辆 位 置 ， 曲 线 e 为 预 
测 时 域内 车 辆 位 置 。 从 图 3.11 可 以 看 出 ,车辆 在 模型 预测 控制 器 的 作用 下 
快速 跟踪 上 了 期 望 轨迹 ， 最 后 能 够 沿 着 期 望 轨迹 稳定 行 怠 ， 达 到 了 预期 效 
R; 同时 ， 我 们 通过 这 张 图 也 能 直观 看 出 模型 预测 控制 中 滚动 优化 的 含义 ， 
即 在 每 一 采样 周期 都 会 预测 系统 在 一 段 时 域 的 状态 信息 ， 进 而 通过 优化 求解 
得 到 一 个 控制 序列 。 另 外 ， 我 们 也 应 当 注 意 到 一 个 细节 ， 就 是 车 辆 实际 经 过 
的 位 置 与 预测 的 位 置 并 不 重合 。 这 主要 是 因为 采用 了 线性 误差 模型 作为 预测 
模型 ， 而 车 辆 位 置 的 解 算 采用 的 是 非 线性 运动 学 模型 。 这 种 现象 在 实际 应 用 
中 普遍 存在 ， 因 为 预测 模型 往往 不 是 对 被 控 系 统 的 精确 建 模 ， 而 是 对 系统 的 
合理 简化 。 


跟踪 结果 对 比 


N 
a w 


纵向 位 置 Y/m 
ご pue t 
e un 一 un N 





3 - 5 


= 0 1 2 
横向 位置 X/m 


[3.11 实例 运行 结果 


为 了 进一步 了 解 系统 运行 过 程 中 其 他 状态 量 的 情况 ， 可 以 通过 以 下 绘图 程 
序 进行 : 





系统 状态 量 随时 间 变 化 

figure (2) 

subplotQ,1,1): 

plot (Tout (1 :Nr),Xout (1 :Nr,1)); 
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hold on; 

plot (Tout (1:Nr),x real (1:Nr,1),'r'); 
xlabel (‘采样 时 间 T’); 

ylabel (‘横向 位 置 X’) 

subplot (3,1,2); 

plot (Tout (1 :Nr),Xout (1 :Nr,2)); 

hold on; 

plot (Tout (1:Nr),x real (1:Nr,2),'r'); 
xlabel (‘采样 时 间 T’); 

ylabel (' 纵 向 位 置 Y') 

subplot (3,1,3); 

plot (Tout (1:Nr),Xout (1:Nr,3)); 

hold on; 

plot (Tout (1:Nr),x real(1:Nr,3),'r*'); 
hold on; 

xlabel (‘采样 时 间 T'); 

ylabel (‘\theta’) 


figure (3) 

subplot (2,1,1); 

plot (Tout (1:Nr),u, real (1:Nr,1),'r*); 
xlabel (C'FÉBT[8] T’); 

ylabel (' 纵 向 速度 ') 

subplot (2,1,2) 

plot (Tout (1 :Nr),u, real (1 :Nr,2),'r*); 
xlabel (' 采 样 时 间 T"); 

ylabel (' 角 速度 ') 

人 ES 
を 状态 量 俩 差 随 时 间 变 化 


figure (4 ) 
subplot (3,1,1); 
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plot (Tout (1:Nr),x piao(l:Nr,1)); 
xlabel (' 采 样 时 间 T’); 
ylabel ('e (x)'); 

subplot (3,1,2); 

plot (Tout (1:Nr),x. piao (1 :Nr,2 ) ) : 
xlabel (‘采样 时 间 T'); 

ylabel ('e(y)'); 

subplot (3,1,3); 

plot (Tout (1 :Nr ) ,x_ piao(1:Nr,3)); 
xlabel (' 采 样 时 间 T’); 

ylabel ('e(NXheta)'); 

s 程序 结束 


绘图 结果 如 图 3.12、 图 3.13 和 图 3. 14 所 示 。 其 中 图 3. 12 中 虚线 为 参考 
状态 量 ， 而 实 线 为 跟踪 过 程 中 实际 的 控制 量 。 从 图 中 可 以 看 出 ， 无 人 驾驶 车 辆 
在 存在 初始 误差 的 情况 下 能 快速 消除 误差 并 最 终 达到 稳定 状态 。 
E 
E g 0 ーー 
E, *-04 
i 70 2 3 4 5 0 1 2 3 4 5 
E 采样 时 间 7/s 采样 时 间 T/s 
E 4 g * 
ー 06 I 3 5 ubi cS ue c sERC MGEDO 
A 采样 时 间 7/s _ 采样 时 间 7/s 
| 
6 1 » 20 1 2 3 4 5 
采样 时 间 T/s 采样 时 间 T/s 
图 3.12 系统 状态 量 随时 间 变 化 图 3.13 状态 量 偏差 随时 间 变 化 
p^ 2 $ 1 
E [: 
B 1.5 — 0 
E 1 B -! 
sw 0 1 2 3 4 ss 0 1 2 3 4 5 


采样 时 间 T/s 采样 时 间 T/s 
图 3.14 控制 量 随 时 间 变 化 
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o 
3.4 TEPER Bl dri dil 1 14 


3.4.1 问题 描述 


在 上 面 的 实例 中 ， 为 了 应 用 线性 时 变 模 型 预测 控制 ， 对 一 个 非 线性 车 辆 运 
动 学 模型 进行 了 线性 化 。 相 对 于 非 线 性 模型 预测 控制 ， 线 性 时 变 模 型 预测 控制 
是 一 种 次 优 的 选择 。 随 着 研究 的 深入 与 硬件 水 平 的 提高 ， 非 线性 模型 预测 控制 
的 应 用 范围 也 愈加 广阔 
对 于 一 个 非 线性 系统 ， 考 虑 如 下 一 般 形式 的 离散 模型 ; 
を (7 + 1) =/( を (7/) ,u(t)) 
を (/) eX, ul) e P 
式 中 , AC, ) AIRBAGS ERSERR. を n, 维 状态 变量 ，z 为 m, 维 控制 变 
ut. X 为 状态 变量 约束 , 丁 为 控制 变量 约束 
BEJO, 0) 20 为 系统 的 一 个 稳定 点 ,同时 也 是 系统 的 控制 目标 。 对 于 
任意 的 时 域 Y， 考 虑 如 下 的 优化 目标 函数 JA Cu) : 


(3. 27) 


1+ 六 一 | 
ED . の (7) ) = YAE), u(k)) + ア (を (7 + N)) (3.28) 
kzl 
式 中 、 の (7) =[u(t), =, uQ +N-1)]' 是 在 时 域 N 内 的 控制 量 输入 序列 ， 
EC AE fef Aqu] S94 UCo) 作用 于 系统 下 的 状态 向 量 轨 迹 ， 优 化 目标 函数 中 
的 第 一 項 /(. ・) 表征 对 期 望 输出 的 跟踪 能 力 ， 第 二 项 PC) 表征 终端 约束 。 


结合 式 (3.27) 和 式 (3.28) ， 非 线性 模型 预测 控制 就 是 要 在 每 一 个 步 长 
内 求解 以 下 带 约 束 的 有 限时 域 优 化 问题 : 

4, P850, ibi D (3. 29a) 

を た Gur = S. ko ty = (3. 29b) 

&,eX hkstel,-,t*N-1 (3. 29c ) 

g,, er k=l tN- (3.29d) 

Sr = &) (3. 29e) 

る € Xm (3.29f) 


其 中 ， 式 (3.29b) 为 系统 所 决定 的 状态 约束 ， 式 (3.290) 和 式 (3.294) 分 别 
为 状态 向 量 和 控制 输入 向 基 约 束 ， 式 (3.29e) 为 初始 状态 约束 ， 式 (3.29f) 为 


久 端 状 在 釣 東 
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假定 上 述 优化 问题 存在 可 行 解 ， 通 过 求解 该 问题 ， 可 以 得 到 最 优 控制 序列 
U; (H) =[ui,,，…， Uw-1w]。 根 据 模型 预测 控制 的 原理 ， 只 是 将 该 控制 序列 
中 的 第 一 个 元 素 作为 受 控 对 象 的 实际 控制 输入 ， 即 : 

u(£(1)) = u,, (3.30) 

在 下 一 个 采样 时 刻 ， 系 统 重 新 以 新 的 采样 时 刻 为 初始 ibn gu 
x (3.29), ， 并 继续 将 控制 序列 的 第 一 个 元 素 施加 给 受 控 对 象 ， 如 此 循环 ， 了 
至 完成 整个 控制 过 程 。 对 于 任意 的 非 线性 模型 预测 问题 ， ei ee 
程 中 包含 了 N(n+m) ARRE, n 个 非 线 性 状态 约束 ; 同时 ， 也 包含 了 
由 控制 输入 约束 和 状态 向 量 约束 决定 的 一 系列 线性 约束 。 因 此 ， 对 于 非 线性 模 
型 预测 控制 ， 其 求解 难度 会 随 着 系统 状态 维度 的 增加 而 迅速 增加 。 对 于 阶 次 较 
低 的 非 线性 系统 ， 如 车 辆 3 自由 度 运动 学 模型 ， 非 线性 模型 预测 控制 的 优势 依 
RFE: 而 对 于 阶 次 较 高 的 非 线 性 系统 ， 如 车 辆 多 自由 度 动力 学 模型 ， 不 进行 
适当 的 简化 ， 就 很 难 实现 在 线 实时 控制 。 


3.4.2. 非 线性 模型 预测 控制 的 数值 解法 


在 非 线 性 模型 预测 控制 中 ， 通 过 非 线性 模型 、 当 前 的 状态 量 和 控制 时 域 
内 的 控制 量 序列 对 未 来 的 状态 量 进行 预测 。 显 然 ， 这 是 一 个 迭代 的 过 程 ， 由 
于 控制 序列 不 可 知 ， 就 需要 找到 一 个 显 式 的 迭代 方程 对 微分 方程 进行 近似 求 
解 。 在 工程 实际 中 ， 使 用 较为 广泛 的 数值 解法 有 欧 拉 法 (Euler Method). 和 4 
阶 龙 格 - 座 塔 (Runge - Kutta) 算法 。 

对 于 微分 方程 y =f, y) ， 利 用 单 步 的 欧 拉 法 ， 可 以 得 到 n+1 时 刻 相对 
于 nn 时刻 的 迭代 方程 为 : 





ym = Fa + ty) (3.31) 
AP, h 为 迭代 步 长 。 
单 步 的 欧 拉 法 形式 简单 ， 但 精度 较 低 。 当 应 用 场合 需要 较 高 的 精度 时 ， 通 
常 采用 4 阶 龙 格 - 库 塔 算法 。 其 一 般 表述 形式 为 : 


ya = Ya +EUR 2k + 2ks + ka) (3.32) 
式 中 , k = E 二 E. ha た =f h h 
r , |f, Yad s TUE Yn キ っ J fnt Y» + っ Jr 


k, 2f(t, +h, y, * hk.) 
4 阶 龙 格 - PEIS TEUER UON h Nr. RUSO N h 阶 。 和 迭代 步 长 
越 小 ， 计 算 精度 越 高 ， 但 计算 速度 也 会 下 降 。 
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3.4.3 工程 实例 


本 节 依 然 以 无 人 驾驶 车 辆 的 轨迹 跟踪 问题 作为 应 用 背景 。 读 者 在 阅读 过 程 
中 可 以 把 两 种 方法 进行 对 比 ， 这 样 就 可 以 更 加 深入 理解 两 种 算法 的 区 别 。 

首先 我 们 单独 定义 一 个 m 函数 (chapter3_ 4_ 3. m) ， 用 于 计算 非 线 性 模 
型 预测 控制 的 目标 函数 ， 命 名 为 MY_ costfunction。 在 这 个 函数 里 ,我 们 需要 
根据 车 辆 的 当前 状态 信息 和 未 来 的 控制 序列 去 预测 车 辆 的 未 来 状态 信息 ， 并 计 
算 目 标清 数值 。 传 递 参数 设置 为 待 求 状态 量 、 当 前 状态 量 、 预 测 时 域 、 控 制 时 
域 、 采 样 周期 、 参 考 轨 迹 和 权重 矩阵， 包含 了 我 们 设计 非 线 性 模型 预测 控制 器 
所 必需 的 参数 。 程 序 如 下 : 





function cost -MY costfunction (x,State Initial,Np,Nc,T, 
Xref,Yref,PHIref,Q,R) 





cost =0; を 初 始 化 目 本 画数 人 
121; e 车 辆 轴 距 
を 矩阵 初始 化 ,包含 当前 状态 .预测 状态 ,状态 偏差 和 控制 量 


X-State Initial(1,1); 
Y-State Initial (2,1); 
PHI-State Initial (3,1); 


X predict -zeros (Np,1); 
Y predict -zeros (Np,1); 
PHI, predict - zeros (Np,1); 


X error - zeros (Np-41,1); 
Y error - zeros (Np+1 ,1 ) : 


PHI error -zeros(Np-«1,1); 


v - zeros (Np,1); を 控制 量 v 
delta f -zeros(Np,l); を 控制 量 delta_f 
$ ニニ ニニ ニニ ニニ ニニ ニー ニー ニニ = ニテ ニニ ニニ ニニ ー ニ ニニ ニニ ニー ニー ニニ ニニ ニニ ニニ ニニ ニー ニニ ニニ ニー ニー ニー ニニ ニニ ニニ ニニ ニニ ニー 
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s 状态 更 新 
$ ============================================================= 
fori-z1:1:Np 
i£ isl 
v (X 1)-x(1); 
delta f(i,1)2x (2); 
X predict(i,l1)2X-« T* v(i,l)*cos (PHI); 
Y predict(i,l1)-Ye« T*v(i,1l)*sin (PHI); 
PHI, predict(i,1)- PHI«T*v(i,l)*tan(delta 
£(1,1))/15; 
else 
v (i,1)2x(3); 
delta, f (i,1)2x(4); 
X predict(i,1)- X, predict(i-1)-«T*v(i,l)*cos 
(PHI. predict (i -1)); 
Y predict(i,1)-Y. predict(i-1)«-T*v(i,l)*sin 
(PHI. predict (i-1)); 
PHI predict(i,1)2 PHI, predict(i-1)«- T*v(i,l) 
* tan(delta f(i,1))/1; 
end 
名 ニニ ニー ニー ニニ ーー ニー ニニ ーーー ニ ニニ ニー ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニー ニニ ニニ ニー ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニ 
% 计算 预测 时 域内 的 轨迹 偏差 
$ ============================================================= 


X_rea] = zeros (Np +1,1); 

Y_real = zeros (Np +1,1); 

X real(1,1)-X; 

X real(2:Np-«1,1)-X predict; 

Y xeal(L,l)es Y; 

Y real(2:Np-«1,1)-Y predict; 

X error(i,1)-X real(i,1)-Xref(i,1); 
Y error(i,1)-Y real(i,1)-Yref (i,1); 


PHI error (i,1)- PHI, predict (i,1)- PHIref (i,1); 
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cost =cost +Y_error *R *Y_error +X_error *Q *X_error; 

€ 程序 结束 

有 了 上 面 程序 的 基础 ,对 于 非 线 性 模型 预测 控制 方法 的 处 理 就 会 简单 很 多 。 
关键 步骤 仍然 和 3.3.3 节 中 介绍 的 一 样 , 首先 给 定 系统 的 基本 参数 , 然后 生成 参 
区 轨迹 ,最 后 对 日 标清 数 进行 求解 。 为 了 动态 表示 无 人 驾驶 车 辆 对 直线 轨迹 的 跟 
踪 过 程 , 就 需要 在 求解 之 后 实时 绘制 车 辆 的 位 置 , 程序 (chapter3_ 4_ 3.m) 如 下 : 





clc; 


clear all; 


E 一 一 一 人 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 人 一 一 一 一 一 一 一 
s 参数 初始 化 

名 ーー トー 
Nx =3; $ 状态 量 个 数 

Np 215; e 预测 时 域 

Nc -2; s 控制 时 域 

1z1; 2 车 辆 轴 距 


tate Initial-zeros (Nx,1); s 状 在 年 隆 初 始 化 


5 
State Initial (1,1)20; 
Q 


% x 
tate Initial (2,1)20; $y 
State Initial G,1)-pi/6; $ phi 
Q-100 * eye(Np+1,Np+1 ) : & 权重 矩阵 
R=100 * eye (Np +1,Np +1); る 权重 算 阵 
2 4G E " 
N =100: e Esc 
T-0.05; る 采样 周期 


Xref -zeros (Np,1); 
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Yref -zeros (Np,1); 

PHIref - zeros (Np,1); 

for Nref =1 :1 :Np 
Xref (Nref,1)-(j«Nref-1)*T; 
Yref (Nref,1)-2; 
PHIref (Nref,1)-0; 


for j21:1:N 
1b2[0.8; -0.44;0.8; -0.44]; 
ubz[1.2;0.44;1.2;0.44]; 
Az[]; 
bz[]; 
Aeq =[] : 
beq =[] : 
options -optimset (‘Algorithm, active -set ^); 
[A, fval,exitflag]- fmincon (8 (x)MY. costfunction (x, 
State Initial,Np,Nc,T,Xref,Yref,PHIref,Q,R),[0;0; 
0;0;],A,b,Aeq,beq,lb,ub,[(],options); gs 有 约束 求解 
v actual-A(1); 
deltaf actual-A(2); 


X00 (1 )= State Initial(1,1); 

X00 (2 )= State Initial (2,1); 

X00 (3)2State Initial (3,1); 

XOUT -dsolve('Dx- v actual * cos (z) 20; Dy - v actual 
*sin(z)-z0;'Dz-v actual * tan (deltaf actual) -0; 'x 
(0) 2X00 (1); y (0) 2 X00 2); /z(0) X003) ); 

tag; 

State Initial (1 ,1 )= eva1 (XOUT. x); 

State Initial (2,1)-eval (XOUT. y) ; 

State Initial (3 ,1 )= eva1 (XOUT. z); 
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© 
figure (1 ) 
plot (State Initial(1,1),State Initial(2,1),'bx ^); 
hold on; 
plot ([0,5],([2,2], r -- ^; 
hold on; 


axis([05 0 4]) 
end 
e 程序 结束 
运行 之 后 的 结果 如 图 3. 15 所 示 。 从 图 3. 15 中 可 以 看 出 ， 无 人 驾驶 车 辆 在 
控制 融 作 用 下 成 功 跟 踪 上 了 期 望 轨迹 ， 并 且 最 后 保持 稳定 。 但 在 局 部 区 域 出 现 
了 超 调 的 现象 ， 而 通过 调整 控制 器 的 预测 时 域 可 以 消除 超 调 。 图 3. 16 是 将 预测 





0 0$ 1 jj à ij 3 まき 4 4$ E 
X/m 


图 3.15 基于 非 线 性 模型 预测 控制 的 直线 轨迹 跟踪 结果 








WU "T 
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时 域 由 15 增加 到 25 时 的 结果 ， 可 以 清楚 地 看 到 ， 已 经 没有 了 超 调 的 现象 。 读 
者 在 编写 程序 的 时 候 ， 也 可 以 多 尝试 改变 控制 器 参数 ， 观 看 结果 ， 这 样 可 以 对 
模型 预测 控制 有 更 加 深入 的 了 解 


3.5 线性 约束 下 的 二 次 型 规划 控制 算法 


线性 二 次 型 调节 需 (Linear Quadratic Regulator, LOR) 的 研究 对 象 是 现代 
控制 理论 中 以 状态 空间 形式 给 出 的 线性 系统 ， 而 目标 函数 为 对 象 状 态 和 (或 ) 
控制 输入 的 二 次 型 函数 。LQR 的 任务 在 于 当 系 统 状 态 由 于 任何 原因 偏离 了 平 
衡 状 态 时 ， 能 在 不 消耗 过 多 能 量 的 情况 下 ， 保 持 系统 状态 各 个 分 量 仍 接近 于 平 
衡 状 态 。LQR 理论 是 现代 控制 理论 中 发 展 最 早 ， 也 最 为 成 熟 的 一 种 状态 空间 
设计 法 。 特 别 可 贵 的 是 ，LQR 解 出 的 控制 规律 是 状态 变量 的 线性 孔 数 ， 因 此 
可 以 通过 状态 反馈 实现 闭环 最 优 控制 。 这 在 实际 工程 应 用 中 具有 重要 的 意义 
3.5.1 线性 约束 转化 为 LOR 问题 | 
XFT cto RerPagze EE, np oS DI H RETR e AH SEE 
法 是 一 种 寻找 变量 受 一 个 或 多 个 条 件 限 制 的 多 元 函数 的 极 值 的 方法 。 这 种 方法 引 
入 了 一 种 新 的 标量 未 知 数 ， 即 拉 格 朗 日 乘 数 。 上 有 具体 实现 方法 如 下 . 
x(1) S f[x(t) ,u(t) .t] (3.33) 
式 中 , *(/) 为 状态 变量 ,，u(1) Hh, flx), u(t), t] 为 连续 可 微 的 
矢 量 函数 。 
IRE telt, n]. WIRE x(4) 2x. PENAS x(t) 自由。 二 次 型 
评价 函数 的 一 般 形 式 如 下 : 
J= FUOC) + ぅ | reco + 4 R(niujdt (3.34) 
IF, の (/) 为 半 正 定 的 状态 加 权 和 矩阵 ，R(1) 为 正定 的 控制 加 权 和 矩阵 ，@,(7) 
为 半 正 定 的 终端 加 权 和 矩阵 。 在 实际 工程 应 用 中 OU) 和 R(1) 是 对 称 矩 阵 ， 常 
Bos fate. 
X (3.34) "Pit ffs 9 x D C)x Gu) 突出 了 对 终端 误差 的 要 求 ， 
叫 作 终端 代价 函数 。 当 对 终端 状态 要 求 比较 严格 时 ， 需 要 加 入 这 一 项 ， 以 体 
现在 t 时 刻 的 误差 足够 小 。 被 积 函 数 中 第 一 项 了， -le'ginx, 5x RRR 
HRE, IA L 表示 跟踪 过 程 中 误差 大 小 的 代价 函数 。 被 积 函数 的 第 二 项 


5rjs.cn 000000 


第 3 章 “模型 预测 控制 算法 基础 与 仿真 分 析 





L, = 了 W'R(1)u， 表 示 动 态 过 程 中 对 控制 的 约束 或 要 求 。L, 是 用 来 衡量 控制 


功率 大 小 的 代价 函数 。 

通过 使 目标 评价 函数 J 取得 最 小 值 ， 寻 求 最 优 控制 ua(1)， 将 系统 从 初始 

态 转 移 到 终端 状态 ， 实 现 以 不 大 的 控制 保持 较 小 的 跟踪 误差 .达到 能 量 和 跟 
"m 综合 最 优 的 目的 . 

为 了 求解 线性 约束 下 评价 函数 / 的 极 值 ， 应 用 拉 格 朗 日 乘 子 法 构造 增 广泛 
Ko 首先 将 状态 方程 写成 约束 方程 的 形式 : 


f[xG),u(t) t] - x(t) 20 (3.35) 
然后 通过 引入 待定 的 拉 格 朗 日 乘 子 矢量 ACO , Fate 函数 : 
J= f. |J + A'[f x(t), u(t),t] 7 x()]1dt (3.36) 


至 此 ， 我 们 将 一 个 含有 线性 约束 的 最 优化 问题 题 转 换 为 一 个 可 以 通过 经 典 变 
分 方法 求解 的 方程 组 (其 变量 不 受 任何 约束 ) 的 极 值 问题 。 

3.5.2 LOQR 在 无 人 驾驶 车 辆 路 径 跟踪 中 的 应 用 

在 无 人 驾驶 车 辆 路 径 跟 踪 的 应 用 中 ， 一 般 利 用 LOR 构建 前 馈 + 反馈 的 控 
制 系统 : 利用 参考 路 径 及 其 曲率 获取 前 馈 控 制 量 ， 并 采用 LOR 消除 无 人 驾驶 
车 辆 当前 状态 与 参考 轨迹 之 间 的 状态 误差 

给 出 由 点 序列 Se= l So, S, e Se) 组 成 的 参考 轨迹 ， 如 图 3.17 所 
Ia nJ AES EJ 率 获 得 前 馈 控 制 量 uy = | uo, uj, cn. uylo HEP 
ü, (ks1, 2, …。 N) 表示 从 点 d | 到 S, (r* nt fos 制 量 ， 


Sy- S. 


V 


期 望 轨迹 点 
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这 里 引用 第 2 章 的 车 辆 运动 学 模型 式 (2.7) 。 由 于 这 是 一 个 非 线性 模型 , 
故 需要 对 其 进行 线性 化 ， 线 性 化 过 程 请 参考 本 书 第 4. 2. 1 节 ， 得 到 线性 化 的 车 
辆 运动 状态 空间 方程 如 式 (3.37 ) 。 由 于 LOR 无 法 处 理 多 变量 控制 过 程 中 的 约 
束 问 题 ， 故 这 里 不 考虑 无 人 驾驶 车 辆 行驶 过 程 中 的 约束 因素 。 

Ax,,, = A, Ax, + B, Au, (3. 37) 

IP, Axr, 表示 年 策 当 前 的 状 恋 量 与 参考 点 的 状 恋 量 的 偏差 , 4, = J / x. 
u,), B, 2J/f(x,, u,). J, 和 J 为 函数 /关于 x Il u 的 Jacobian 矩阵 

使用 LOR 消除 车 辆 当前 状态 与 参考 路 径 上 参考 点 的 状态 误差 ， 首 先 定 义 
如 下 的 评价 函数 : 


J = テス -r,)' Q(x; -r,) + (u, + Au,)'R(u, + Au,)] + 
iG. - r) Qo(t) (xy - ry) (3.38) 
AP, の 为 状态 量 误差 的 权重 矩阵 ，R AFEK EHAE, Qs 为 终端 状态 
REER, x, 为 无 人 驾驶 车 辆 在 第 时刻 的 预测 状态 ，r, 为 参考 路 径 上 第 大 个 
参考 点 的 状态 ，ui, + Au, 为 第 时 刻 的 控制 量 。 
引入 拉 格 朗 日 乘 子 ， 构 造 无 约束 的 最 优化 问题 ， 如 下 : 


y Gy -ny'Qy) x, - ry) 4 


N-I 
iG, -n)'QG, or) + Qu + Au) Ri, + Au) + 
k=0 


Ai (A, Ax, + B,Au, - Ax,4)] (3. 39) 
构造 哈密 尔 顿 函数 ， 今 
H, -l[G, - r)'Q(x, - r,) + (u, + Au,)' R(u, + Au,) + 
Ara (A, Ax, + B,Au,) ] (3. 40) 


将 式 (3.40) 代入 式 (3.39) , 可 得 : 
1 g , E i 
V = 二 (zw n) B (xy - ry) + Y [ H, + AlAr, + Y, (H, - Ajax) | 


(3.41) 
当 评 价 函 数 取 最 小 值 时 ， 函 数 V 需 满足 : 
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O 
aV 
9( Ax, ) SM 
bas ^ (3.42) 
Wk a 
9( Ar、) 
由 式 (3.42) 可 得 : 
QAx, +4A = Au (3.43) 
R(u, + Au,) + BiA,,, =0 (3.44) 
QoAxy = A. (3.45) 
根据 式 (3.43), 假设: 
À, = S,AX, + v, (3.46) 


由 式 (3.38), 3È (3.43 -3.45) 和 式 (3.46) 可 得 : 
K = (B,S,,B, + R) "BIS,,,A, 
K, = (BIS, B, -R)"R 
K, = (B'S,,B, + R) !B; puse 
S, = AIS, (A, - BK) +0 
v, = (A, - B,K)v,,, - K'Ru, 
根据 已 知 的 $5、 2 Q,. v, 20 和 终端 状态 xv， 通 过 向 后 迭代 可 以 求 得 反馈 
控制 的 最 优 控制 序列 ， 如 下 式 所 示 ; 


Au, = — KAx,,, - K,u, - K,v,,, (3.48) 
最 终 得 到 无 人 驾驶 车 辆 的 控制 量 为 : 
u” zd, A, (3.49) 


IUP, u, 为 前 馈 控 制 量 ，Aw 为 反馈 控制 量 
3.5.3 LOR 进行 路 径 跟踪 的 工程 实例 


在 工程 应 用 中 ， 首 先 完成 车 辆 的 初始 状态 的 设置 和 系统 参数 的 初始 化 ， 如 
FLA: 








Hp =10: $ i$ finite -horizon 
N 1-200; を BAKE 

Nx -3; る 状态 变量 参数 的 个 数 

Nu =1; $$ 控制 变量 参数 的 个 数 
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一 一 一 
FWA = zeros (N 1,1); gs 前 轮 偏 角 
FWA (1 ,1)=0; 初始 状态 的 前 轮 偏 角 
x_real = zeroS (Nx,N 1); S 实际 状态 
x_real(:,1)=[22 0 pi/2]; % x0 = 车 辆 初始 状态 X_init 初始 状 
态 , 给 了 2 m 的 横向 偏差 
RefTraj = zeros 3,1); を 参考 点 的 状态 
Delta x-zeros (3,1); を 车 辆 当前 状态 与 参考 点 状态 的 偏差 
Q=[1000;0100;00100];  $ 对 状态 量 误差 的 权重 矩阵 
R=[10]; S 对 控制 量 的 权重 矩阵 
Pk-[10 0;010;00 1]; s 人 为 给 定 , 相 当 于 O00 
Vk =[0 0 0] 


根据 参考 路 径 计 算 车 辆 控制 的 前 馈 控 制 量 。 这 里 假设 无 人 驾驶 车 辆 跟踪 一 
段 圆 心 在 原点 、 半 径 为 20 m 的 圆 弧 。 计 算 前 馈 控制 量 的 函数 如 下 : 





Function [RefTraj x,RefTraj y,RefTraj theta,RefTraj _ 
delta]- 
Func, CircularReferenceTrajGenerate (Pos x,Pos, y,CEN x, 
CEN. y,Radius,N,Velo,Ts,L) 
る RefTraj 为 要 生成 的 参考 路 径 ,Pos_x,Pos_y 为 车 辆 坐标 ,CEN_x， 
CEN y,Radius 为 圆心 与 半径 
gs N 要 生成 几 个 参考 点 , 即 预测 空间 ,Velo,Ts 车 速 与 采样 时 间 民 汽车 的 轴 距 
RefTraj = zeros (N,4 ) : gs 生成 的 参考 路 径 
Alpha init -Func Alpha. Pos (CEN x,CEN y,Pos x,Pos y); 
gs 首先 根据 车 辆 位 置 和 圆心 确定 alpha 
Omega =Ve1o/Radius gs 已 知 车 速 和 半径 ,可 以 求 得 角速度 
DFWA = atan (L/ Radius); 
for kz1:1:N 
Alpha (k)- Alpha, init «Omega * Ts * (k-1); 
RefTraj (k,1)-Radius * cos (Alpha (Kk) )» CEN. x;$ x 
RefTraj (k,2)- Radius * sin (Alpha (k))« CEN y;$ y 
RefTraj (k,3)- Func, Theta, Pos (Alpha (k));$ theta 
RefTraj(k,4)- DFWA;  $ 前 轮 偏 角 , 可 以 当 作 前 馈 量 
end 
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RefTraj x-RefTraj(:,1); 
RefTraj y-RefTraj(:,2); 
RefTraj theta -RefTraj(:,3); 
RefTraj delta-RefTraj(:,4); 


end 


然后 计算 消除 跟踪 误差 的 反馈 控制 量 ， 计 算 公 式 如 式 (3.49) 所 示 。 算 
法 突 現 如 下 所 示 、 具体 的 Matlab 实现 m 文件 请 参考 Chapter 3_ 5_ 3.m。 





for isisi N 1 
G Test =3 跟踪 第 几 个 参考 点 ,可 以 应 对 系统 的 延 时 
S 先 确 定 参 考点 和 确定 矩阵 A,B. 这 里 姑且 认为 A 和 B 是 不 变 的 
[RefTraj_x,RefTraj_y,RefTraj_theta, RefTraj_deltal]= Func 
_CircularReferenceTrajGenerate(x_real (l,i),x_real 
(2 ,i ) ,CEN (1 ),CEN(2),Radius,Hp,vel,T,L); 
u feedForward -RefTraj delta (G Test);$ 前 馈 控 制 量 
Delta x(1,1)2x real(1,i)-RefTraj x(G Test); 
Delta x(2,1)2x real (2,i)-RefTraj. y (G Test); 
Delta x(3,1)-x real G,i) -RefTraj theta (G Test); 
if Delta xG,1)»pi % 限制 航向 偏差 的 范围 
Delta_x(3,1)=Delta x(3,1)-2 *pi; 
else if Delta x (3,1) «-1*pi 
Delta x(3,1)2 Delta x(3,1)42 * pi; 
else 
Delta x(3,1)2Delta x (3,1); 
end 
end 
$ 通过 Backward recursion KK 
for j-2Hps-1:2 
Pk 1-Pk; 
Vk. 1 sVk; 
A-[10-vel*sin(RefTraj theta(j-1)) * T;0 1 vel 
cos (RefTraj theta(j-1)) * ;00 1;]; 
COS2 = cos (RefTraj delta(j-1))^2; 
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B=[0 0 vel*xT/(L*COS2)]^; 
K= (B'*Pk 1*A)/(B'*Pk 1*B+R); 
Ku=R/(B'*Pk 1*B-«R); 
Kv-B'/(B'*Pk 1 *B+R): 
Pk=A'*PK ]* (A-B*K)«Q; 
Vk-(A-B*K)'*Vk 1-K'*R*RefTraj delta(j-1); 
end 
u. feedBackward -- K * (Delta, x)- Ku * u, feedForward - Kv 
* Vk 1; 
FWA (i-*1,1)-u, feedForward -«u, feedBackward; 
[x real(l1,i*1),x real(2,i-«1),x real(3,i-«1)]-Frunc. 
VehicleKineticModule Euler (x real (1,i),x real (2, 
i),x real G,i),vel,FWA(i,1),FWA(i-1,1),T,L); 
End 


HP, OU) 通常 是 对 角 线 常 阵 ， 对 角 线 上 的 元 素 分 别 表示 对 应 相应 误差 分 量 
的 重视 程度 。 越 被 重视 的 误差 分 量 , 希望 它 越 小 ， 相 应 的 其 加 权 系 数 就 应 取得 
越 大 。 如 果 对 误差 在 动态 过 程 中 不 同 阶段 有 不 同 的 强调 ， 那 么 相应 地 就 应 取 成 
时 变 的 。 基 于 LOR 的 路 径 跟踪 效果 如 图 3. 18 所 示 。 
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图 3.18 线性 约束 下 基于 LQR 的 路 径 跟踪 
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3.5.4 J 和 睹 


对 于 线性 系统 的 控制 器 设计 问题 ， 如 果 其 性 能 指标 是 状态 变量 和 (或 ) 
控制 变量 的 二 次 型 函数 的 积分 ， 则 这 种 动态 系统 的 最 优化 问题 就 被 称 为 线性 系 
统 二 次 型 性 能 指标 的 最 优 控制 问题 ， 简 称 线性 二 次 型 最 优 控制 问题 或 线性 二 次 
型 问题 . 线性 二 次 型 问题 的 最 优 解 可 以 写成 统一 的 解析 表达 式 ， 实 现 求解 过 程 
的 规范 化 ， 并 可 简单 地 采用 状态 线性 反馈 控制 律 构成 闭环 最 优 控制 系统 ， 能 够 
匡 顾 多 项 性 能 指标 ， 因 上 v 得 到 特别 的 重视 ， 为 现代 控制 理论 中 发 展 较为 成 熟 的 
一 部 分 。 LQR 最 优 控 制 利 用 廉价 成 本 可 以 使 原 系统 达到 较 好 的 性 能 指标 ( 事 
实 也 可 以 对 不 稳定 的 系统 进行 整定 ) ， 而 且 方 法 简单 ， 便 于 实现 ， 同 时 利用 
Matlab 强大 的 功能 体系 容易 对 系统 实现 仿真 。 本 节 详 细 介 绍 了 线性 二 次 型 规划 
器 LQR， 并 介绍 了 在 车 辆 运动 学 模型 的 基础 上 的 LOR 路 径 跟 踪 控 制 器 ， 给 出 
T eR CS PLC 
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本 章 首 先 对 无 人 驾驶 车 辆 的 轨迹 跟踪 问题 进行 了 描述 ， 结 合 模型 预测 控制 
算法 的 特点 提出 了 基于 模型 预测 控制 的 轨迹 跟踪 问题 的 流程 以 及 控制 结构 。 由 
于 无 人 驾驶 车 辆 在 不 同 速度 下 对 轨迹 跟踪 问题 的 要 求 不 同 ， 分 别 基于 运动 学 模 
型 和 动力 学 模型 设计 了 两 类 轨迹 跟踪 控制 器 。 其 中 ， 基 于 动力 学 模型 的 跟踪 控 
制 器 采用 车 辆 横向 动力 学 模型 ， 对 车 辆 进行 主动 转向 控制 。 设计 过 程 中 ， 结 合 
第 2 章 中 的 模型 以 及 第 3 章 的 算法 推导 ， 从 线性 误差 模型 、 约 束 条 件 以 及 控制 
Ar Eb ER CE E 3 方面 进行 设计 

为 了 验证 所 设计 控制 系统 的 有 效 性 与 可 靠 性 ， 选 择 成 熟 的 商用 动力 学 仿真 
软件 CarSim 与 Matlab/Simulink 构建 联合 仿真 平台 。 参 考 现 有 无 人 驾驶 车 辆 平 
台 的 整 车 配置 与 参数 ， 在 CarSim 中 搭建 了 整 车 模型 ， 将 控制 算法 封装 在 白 定 
LAY S 函数 中 ， 实 现 了 无 人 驾驶 车 辆 在 不 同 环境 中 的 轨迹 跟踪 仿真 

最 后 ， 分 别 在 直线 参考 轨迹 和 圆 形 参考 轨迹 下 对 基于 运动 学 模型 的 轨迹 跟 
踪 控 制 器 进行 仿真 。 仿 真 结果 表明 ， 所 设计 的 控制 器 能 够 快速 旦 稳定 地 跟踪 期 
望 轨迹 。 


4.1 问题 的 描述 


无 人 驾驶 车 辆 的 轨迹 跟踪 问题 是 指 根据 某 种 控制 理论 ， 为 系统 设计 一 个 
控制 输入 作用 ， 使 无 人 驾驶 车 辆 能 够 到 达 并 最 终 以 期 望 的 速度 跟踪 期 望 轨 
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YE. 在 懂 性 坐标 系 中 ， 车 辆 必须 从 一 个 给 定 的 初始 状态 。 送 全 初 始点 既 
可 以 在 期 望 轨迹 上 ， 也 可 以 不 在 期 望 轨迹 上 。 divis 无 人 驾驶 车 辆 
的 轨迹 跟踪 问题 可 以 用 图 4. 1 表示 ， 其 中 期 望 轨迹 用 一 个 个 离散 的 轨迹 点 给 
出 。 期 望 轨迹 是 指 一 条 几何 曲线 /(x,(1) ) 、 自 変量 x, 是 时 间 上 的 函数 ， 曲 线 
方程 是 4 的 隐 哺 数 。 给 定 的 速度 是 指 广义 的 速度 变量 ,也 即 参 考 控制 输入 ， 
包括 速度 、 角 速度 和 前 轮 偏 角 等 ,用 uw, 表示 。 7/(r,(7) ) RI u, 既 可 以 由 轨迹 
siepe 也 可 以 预先 设 定 。 在 本 章 中 ， 设 定期 望 轨迹 和 参考 控制 输入 


HAN 
经 被 给 出 





图 4.1 无 人 驾驶 车 辆 轨迹 跟踪 示意 


根据 本 书 的 研究 内 容 ， 进 一 步 对 无 人 驾驶 车 辆 的 轨迹 跟踪 问题 做 出 如 下 限 
定 : 车 辆 为 前 轮转 向 车 辆 ， 而 且 系 统 能 够 有 效 地 提供 两 类 信息 

L 可 行驶 区 域 的 几何 描述 、 路 面 特征 及 路 面 摩擦 系数 ， 

2) 藻 辆 位 置 及 内 部 状态 ， "m 1 纵横 问 速 度 、 加 速度 、 轮 速 等 参数 

这 就 是 说 轨迹 跟踪 控制 是 在 周围 环境 及 车 T 态 完 全 已 知 的 情况 下 进 
行 的 ， 不 涉及 环境 感知 和 和 车辆 状态 的 估计 。 根据 上 述 设 定 ， 基 于 模型 预测 控制 
的 轨迹 跟踪 过 程 如 网 4. 2 所 示 

轨迹 跟踪 过 程 中 所 采用 的 模型 预测 控制 算法 如 网 4.3 所 示 。 其 中 虚线 框 表 
示 的 是 模型 预测 控制 器 的 主体 ， 主 要 由 线性 误差 横 型 、 系 统 约束 以 及 目标 机 数 
组 成 。 线 性 误差 方程 是 轨迹 跟踪 控制 系统 的 数学 描述 ， 也 是 构建 控制 算法 的 基 
而， 系统 约束 包括 车 辆 执行 机 构 约 东 、 控 制 量 平 潜 约 束 以 及 车 辆 稳定 性 约束 
等 ， 目 标 函 数 的 设计 则 综合 葡 虑 轨迹 跟踪 的 快速 性 以 及 平稳 性 。 以 下 从 这 3 方 


面 分 别 介绍 基于 运动 党 借 刚 的 轨迹 跟 中 控制 兰 的 设计 ， 
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MPC 控 制 器 去 跟踪 
轨迹 上 的 点 






参考 轨迹 离散 化 








图 4.2 轨迹 跟踪 流程 


一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 干 一 一 一 = 








下 个 时 刻 
车 辆 状态 


LU 
上 
I 
I 
1 
LI 
1 
I 
1 


控制 量 ， 


图 4.3 “基于 模型 预测 控制 的 轨迹 跟踪 控制 器 
4.2 ”基于 运动 学 模型 的 轨迹 跟踪 控制 占 设 计 
4.2.1 车 辆 运动 学 建 模 
为 方便 阅读 ， 将 第 2 章 车 辆 简化 后 的 运动 学 模型 图 2.1 移 至 第 4 se. 


图 4.4 所 示 。 
在 地 面 固定 坐标 系 OXY 下 ， 车 辆 运动 学 方程 为 : 
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coso 
y |» bad" (4.1) 
- tan0 


r 


e l 
式 中 : (x, 3) 为 车 辆 后 轴 中 心 的 坐标 ，w 
方 年 体 的 航 向 角 , 8 为 前 轮 偏 角 ,4 为 车 辆 
后 轴 速 度 ，! 为 轴 距 
由 式 (4.1) 可 知 ， 系 统 可 以 被 看 作 一 





个 输入 为 u(r, 8) 和 状态 量 为 X(x,，y， 图 4.4 车辆 运动 学 模型 
e) 的 控制 系统 。 其 一 般 形式 为 : 
X = f(X ,u) (4.2) 


对 于 给 定 的 参考 轨迹 ， 可 以 由 参考 车 辆 的 运动 轨迹 描述 。 其 上 的 每 一 个 点 
都 满足 上 述 运动 学 方程 ,用 r 代 表 参 考量 ， 一 般 形式 为 : 
X. = f(x,.u,) (4.3) 
XUP:Xe-[x xy, el'u-[w 8] 
将 式 (4.2) 在 参考 轨迹 点 采用 泰勒 级 数 展开 并 忽略 高 阶 项 ， 得 到 : 














第 = fiu) « OG | (oy, + | (u-u) (4.4) 
9 ギヤ アニ Wr ou de 
将 式 (4.4) 与 式 (4.3) 相 减 ， 可 以 得 到 : 

UE ep, O 
x-x 0 0 -vsing,][ x - x, に 

i l a : sing, 0 [. 一 | 

- =% |= v Coso, |y 

d 5 uà, n [3-3 
e - の , 0 0 0 P= e l l cos’6 


r 


(4.5) 
È (4.5). 即 线性 化 的 无 人 驾驶 车 辆 误差 模型 。 为 了 能 够 将 该 模型 应 用 于 模型 
陆 测 控制 器 的 设计 ， 对 式 (4.5) 进行 离散 化 处 理 : 
X(k+1) = A xX(k) + B,,u(k) (4.6) 
cose, T 0 


1 0 -v sing,T A 

sing, T 0 T 为 采样 时 间 
( ^, COS T E "ium ’ Hx - If IH] 。 
iii oc ' tanó,T v T 


0 0 l / lcos'8, 
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4.2.2 目标 函数 设计 


目标 函数 要 能 够 保证 无 人 驾驶 车 辆 快速 且 平 稳 地 追踪 期 望 轨迹 ， 就 震 要 加 
和 人 对 系统 状态 量 的 偏差 和 控制 量 的 优化 。 文 献 [50] 在 设计 轨迹 跟踪 控制 名 
时 ,采用 如 下 形式 的 目标 函数 : 


N " 
J() = YX G jl )QYG +j) *ü' (kj - D) Ra(kj - 1) 
j=l 


(4.7) 
式 中 ，Q 和 RR 为 权重 矩阵 。 

第 一 项 反映 了 系统 对 参考 轨 线 的 跟随 能 力 。 第 二 项 反映 了 对 控制 量变 化 的 
约束 。 该 目标 函数 的 优点 在 于 容易 转换 成 标准 二 次 规划 形式 ， 但 是 也 存在 比较 
明显 的 缺陷 : 无 法 对 每 个 采样 周期 内 的 控制 增 量 进行 限制 ， 即 无 法 避免 被 控 系 
统 控 制 量 突变 的 现象 ， 从 而 影响 控制 量 的 连续 性 。 参 照 文献 [72] 所 使 用 的 
KERTAS 采用 如 下 形式 的 目标 函数 : 


N.-1 


J(k) = $ nk iD aki De X HAUG ei D I 


(4.8) 
式 中 ，N, A Bit. N. 为 控制 时 域 ; p 为 权重 系数 ; e 为 松弛 因子 。 
AMIX (4.7), X (4.8) 用 控制 增 曾 量 取 代 控 制 量 并 且 加 入 了 松弛 因子 
这 样 不 仅 能 对 控制 增 量 进行 直接 的 限制 ， 也 能 防止 执行 过 程 中 出 现 没有 可 行 解 
的 情况 。 在 目标 函数 中 ,需要 计算 未 来 一 段 时 间 系 统 的 输出 。 以 下 重点 介绍 如 
何 基于 无 人 驾驶 车 辆 线性 误差 模型 预测 车 辆 未 来 时 刻 的 输出 : 
将 式 (4.6). 做 如 下 转换 : 


FI 2) s [4 9 | (4.9) 
u(k- 111) 
得 到 一 个 新 的 状态 空间 表达 式 : 
E(k 1I 1) =4 を (た 17) + B,,AU(EI t) 
(417) = C, を (た 1 の) (4. 10) 


- A Nha . B, , 
stp, =|。 | 5,, = | | n 为 状态 量 维度 ，m 为 控制 量 维度 


为 了 简化 计算 ， 本 书 做 出 如 下 假设 : 
A, 2A,, k=1,,t+N-! 
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O 
B,, = B,,, k 21,4 -N-1 (4. 11) 
经 过 推导 ， 可 以 得 到 系统 的 预测 输出 表达 式 ， 
Y(t) = W£(tlti) + O,AU(I (4. 12) 
式 中 : 
n(t+111) CA,, 
nlt 211) CE, 
Y(t) = z 二 
nt - N. 1 t) C A~“ 
uie C ÀY 
C, B,, 0 0 0 
-- 。 C, B, I 0 0 
Au(tl t) 
Au(t - 1 t) 
9 -|CAVB, GAVB, o 6, AU =| IPC 
C AYB, CA ges C 4, 万 ， Au(t + N, | t) 


CATI, CATI 7 CATO, 


将 式 (4.12) 代入 式 (4.8)， 即 可 得 到 完整 形式 的 目标 函数 
4.2.3 ”约束 条 件 设计 
本 书 主要 考虑 控制 过 程 中 的 控制 量 极 限 约束 和 控制 增 量 约束 。 控 制 量 表达 
形式 为 : 
)zu(t*k) € uj (lt+k), 
k 20,1,-,N, -1 (4.13) 
控制 增 量 约束 表达 形式 为 : 
Au, LC +k) € Ault +k) < Aun (ttk), 
k 20,1,*-,N, -1 (4.14) 
tk HERRUP, RAEI HE ki Ja dap Xp PE ue, RE H fë 
PERY ie ey, e Ae PE da d hk 1 4 6p E REA E ER BL AIE, is xt 
式 (4.14) Jet, cR PH Og ROME. P3 (éd PX: 
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© 
u(t +k)  u(t*k-—1) * Au(t +k) (4. 15) 
Du 

U, = 1, GQQu(k - 1) (4. 16) 

l| p 0 

I 1 0 -— 9 
Asli t 4 '. (| (4. 17) 

Pod v0 

p l1 o Tq 1j 





式 中 : 1。 是 行 数 为 N. 的 列 向 量 , 7。 是 维度 为 m 的 单位 和 矩阵， 的 为 克 罗 内 克 积 
( Kronecker product), u(k -1) 为 上 一 时 刻 实 际 的 控制 量 。 
结合 式 (4.15) ~ 式 (4.17) ， 可 以 将 式 (4.13). 转换 为 以 下 形式 : 
U- SAAV. & U, sz U., (4. 18) 
式 中 : Ui, ，U, 分 别 为 控制 时 域内 的 控制 量 最 小 值 、 最 大 值 集合 ， 
将 目标 函数 转化 为 标准 二 次 型 形式 并 结合 约束 条 件 ， 解 决 以 下 优化 问题 : 
7( を (の ,u(t - 1) ,AU(CO) = [AU(t)",e] "H, [AU(1)",e] + G[AU(GO' ,el] 


(4. 19) 
«t£ AU... < AU, < AU nax 
U nin € AAU, + U, S U,,, 
0/00, +R 0 - 
式 中 : H, = 25 ，G, -[26/00, 0], e, 为 预测 时 域内 的 跟踪 误差 。 
p 


在 每 一 控制 周期 内 完成 对 式 (4.19) 的 求解 后 ， 得 到 了 控制 时 域内 的 一 
系列 控制 输入 增 量 : 


AU; -— [ Au, Au At] (4. 20) 
将 该 控制 序列 中 第 一 个 元 素 作 为 实际 的 控制 输入 增 量 作 用 于 系统 ， 即 : 
u(t) = u(t-1) + Au; (4. 21) 


进入 下 一 个 控制 周期 后 ， 重 复 上 述 过 程 ， 如 此 循环 实现 了 对 车 辆 的 轨迹 跟 
踪 控 制 。 

约束 条 件 中 涉及 的 具体 数值 则 需要 通过 下 面 介绍 的 纵横 向 跟踪 能 力 测试 获得 

在 无 人 驾驶 车 辆 纵向 跟踪 能 力 实验 中 ， 分 别 对 无 人 驾驶 车 辆 输入 5 m/s, 
7.5 m/s 和 10 m/s 的 阶 路 期望 速度 ， 平 稳 行驶 一 段 时 间 后 再 以 最 大 制 动 能 力 对 
车 辆 进行 制 动 ， 记 录 加 速 和 减速 时 间 ， 测 试 结果 如 表 4.1 所 示 。 可 以 看 出 ， 车 
辆 在 中 低速 (7.5 m/s 以下) 加 减速 过 程 较为 平稳 ， 加 速度 维持 在 1 m/s 左 
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右 。 为 了 使 跟踪 过 程 更 加 平稳 ,无 人 驾驶 车 辆 速度 控制 约束 可 设置 为 : 
-0.2m/szv-v, € 0.2 m/s 
— 0.05 m/s < Av x 0. 05 m/s (4. 22) 
Hp. v, 为 车 辆 期 望 速度 ，Az 为 每 个 控制 周期 的 速度 增 量 。 


表 4.1 无 人 驾驶 车 辆 纵向 跟踪 能 力 测试 
项 目 加 速 时 间 t/s 制 动 时 间 t/s 


0~5 m/s 5 








0-7.5 m/s 8 
0 -10 m/s 15 





f B IR ER Efe 71 Sz Su rp, E ABE RU FE A A DDR [vr EEE SE F I] Er DIR 
置 ， 记 录 相 应 的 位 置 和 时 间 ， 测 试 结果 如 表 4.2 所 示 。 通 过 计算 可 知 ， 方 向 盘 
转 一 圈 大 约 需 要 1.8 s， 对 应 于 前 轮 偏 角 为 17*。 因 此 ， 将 车 辆 极限 前 轮 偏 角 和 
前 轮 偏 角 增 量 设置 为 : 
-25?z8 x25? 
- 0.47* < Aó < 0. 47^ (4.23) 


表 4.2 无 人 驾驶 车 辆 原 地 转向 性 能 测试 
项 目 逆 时 针 方 向 顺 时 针 方 向 
方向 盘 极 限 转角 a/(*) -511 492 








对 应 前 轮 偏 角 8/(?) 25 25 
所 用 时 间 //s 4.9 4.9 





结合 式 (4.22) 和 式 (4.23) ， 可 以 得 到 控制 量 的 约束 条 件 为 : 


控制 量 约束 [eme « [7] 
i EI 


4.3 仿真 平台 概述 


本 章 所 构建 的 联合 仿 直 平台 是 包含 车 辆 动力 学 模拟 、 控 制 系统 建 模 和 动画 
演示 的 统一 平台 。 日前， 控制 系统 的 建 模 与 开发 大 多 是 在 Mallab/Simulink f 
境 中 完成 的 。 在 该 环境 中 ， 开 发 人 员 可 以 利用 图 形 建 模 功 能 编写 复杂 的 控制 敢 
4t. 同时 ， 世 界 上 主流 动力 学 仿 直 与 分 析 软 件 都 提供 了 与 Simulink 仿真 的 接 
口 ， 所 以 ， 我 们 选择 Simulink 环境 进行 控制 系统 建 模 
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为 了 满足 车 辆 动力 学 模型 精度 的 要 求 ， 快 速 有 效 地 验证 轨迹 跟踪 控制 算 
法 ， 需 要 选用 成 熟 并 且 具 备 高 精度 的 车 辆 动力 学 仿真 软件 。 符 合 条 件 的 整 车 动 
力学 仿真 软件 主要 包括 美国 MSC 公司 的 CarSim， 德 国 dSPACE 公司 的 ASM, 
加 拿 大 CM - LABS 公司 的 Vortex 等 。 由 于 CarSim 同时 具备 了 车 辆 动力 学 仿真 
与 三 维 动画 演示 功能 ， 其 扩展 包 CarSim RT 提供 了 与 硬件 实时 系统 的 接口 ， 十 
分 适合 无 人 驾驶 车 辆 控制 系统 的 开发 与 测试 。 因 此 ， 选 用 CarSim 作为 车 辆 动 
力学 仿真 软件 ， 版 本 为 8.02。 

综合 上 述 分 析 ， 联 合 仿真 平台 是 以 汽车 动力 学 仿真 软件 CarSim 和 控制 系 
统 仿真 软件 Matlab/Simulink 为 基础 开发 的 。 为 了 充分 结合 两 者 的 优势 ， 震 要 
在 CarSim 中 建立 准确 的 整 车 高 精度 模型 ， 在 Simulink 中 搭建 控制 器 。 最 后 ， 
通过 系统 的 输出 和 性 能 指标 验证 运动 规划 和 控制 算法 的 各 项 性 能 


4.3.1 CarSim 软件 介绍 


CarSim 是 由 美国 MSC 公司 (Mechanical Simulation Corporation, MSC) 开发 
(54482) 77^: EENE CHJ 7C AE RTT EARR E a is fi UT PUT 
( University of Michigan Transportation Research Institute, UMTRI) 多 年 理论 和 实践 
经 验 的 积累 。CarSim 可 以 方便 灵活 地 定义 试验 环境 和 试验 过 程 ， 准 确 预测 和 仿 
真 汽车 整 车 的 操纵 稳定 性 、 动 力 性 、 平 顺 性 等 ， 适 用 于 轿车 、 轻 型 货车 等 车 型 的 
建 模仿 真 。 其 主要 功能 界面 如 图 4. 5 所 示 。 从 图 4.5 可 以 看 出 ，CarSim 主要 包含 
3 大 功能 模块 由 模型 和 工 况 参数 设 定 ; 2 数学 模型 求解 ， 3 输出 和 后 处 理 





车 辆 参数 设 定 主 界面 动画 演示 











图 4.5 CarSim 功能 与 界面 示意 
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(1) 模型 和 工 况 参数 设 定 
该 功能 模块 主要 包括 车 辆 参数 设 定 与 车 辆 测试 工 况 设 定 两 部 分 。 其 中 车 辆 
参数 设 定 需要 从 已 有 的 整 车 模型 数据 库 中 选择 合适 的 车 型 ， 并 对 动力 传动 系 
统 、 悬 架 系 统 等 进行 设置 。 车辆 测试 工 况 设 定 主要 是 对 车 辆 行驶 环境 及 相关 因 
zn — xr. 包括 路 面 信息 、 风 阻 等 外 部 环境 信息 。 
) 数学 模型 -~ 
nhi 块 是 整个 软件 求解 运算 的 内 核 ， 需 要 设置 求解 器 类 型 、 仿 真 步 
长 和 仿真 时 间 — 同时 ， 该 模块 也 是 与 其 他 所 有 外 部 环境 的 接口 ， 包 括 
Simulink, Labview 和 dSPACE 等 
(3) 输出 和 后 处 理 部 分 
该 功能 模块 包括 仿真 结果 的 3D 动画 显示 和 数据 绘图 两 部 分 。 用 户 可 以 通 
LE 走动 画 窗 口 直 观 地 观察 汽车 动力 学 响应 ， 还 可 以 在 数据 绘图 部 分 有 选择 地 
省 出 指定 参数 的 曲线 ， 进 行 定量 分 析 . 


4.3.2 Simulink/CarSim 联合 仿真 平台 


(1) 整 车 模型 搭建 
营建 仿 直 平 台 的 第 一 步 是 在 CarSim 中 搭建 整 车 模型 。CarSim 采用 多 体 动 
力学 建 模 方 法 ， 对 车 辆 进行 了 适当 的 抽象 简化 。 在 模型 搭建 的 时 候 只 需要 依据 
目标 车 型 参数 依次 配置 车 辆 各 子 系统 参数 ， 如 传动 系统 、 制 动 系统 等 。 其 详细 
构成 如 图 4.6 所 示 





[44.6 — CarSim 咎 车 模 剧 包括 的 子 系统 
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为 了 方便 后 续 控 制 算法 向 实 车 实验 的 移植 ， 尽 量 贴近 实际 情况 ， 以 北京 理 
工大 学 智能 车 辆 研究 所 现 有 的 无 人 驾驶 车 辆 平台 一 一 比亚迪 速 锐 2012 3X 
1. STID 自动 旗舰 型 作为 目标 车 型 对 上 述 7 个 子 系统 进行 设置 ， 具 体 配置 如 
* 4.3 所 示 。 





表 4.3 车 辆 模型 主要 配置 


系统 名 称 配置 类 型 





车 体 结构 
助力 类 型 "TE 


Jh vr x 
胎 











制 効 系 ABS 配置 fi 











150 kW ， 直 列 4 kr 
完成 系统 配置 后 ， 还 需要 对 每 项 配置 输入 相应 的 参数 ， 以 实现 对 系统 的 准 
确 描 述 。 涉 及 的 动力 学 模型 参数 如 表 4.4 所 示 。 
表 4.4 动力 学 模型 基本 参数 


























室 气 阻力 系数 
空气 密度 








(2) 仿真 工 况 设 定 
仿真 工 况 设 定 主要 包括 初始 速度 、 驾 驶 员 制 动 模式 、 转 向 模式 、 挡 位 输入 
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和 路 面 参数 等 。 由 于 所 设计 的 轨迹 跟踪 控制 器 实时 向 车 辆 提供 车 轮转 角 信 息 和 
速度 信息 ， 并 且 加 入 了 速度 控制 模块 ， 因 此 制 动 模式 和 转向 模式 均 选择 为 无 ， 
即 不 震 要 模型 和 月 身 施 加 制 动 力 和 转向 力 ， 挡 位 控制 选择 自动 升 挡 。 路 面 参数 需 
要 道路 曲线 、 宽 度 、 摩 擦 系数 等 信息 。 

(3) 外 部 接口 设 定 

主要 包括 连接 需 选 择 、 解 算 器 选择 和 输入 输出 接口 选择 3 部 分 内 容 。 需 要 
在 Simulink 环境 中 进行 仿真 ， 因 此 首先 选择 基于 Simulink 的 连接 器 ， 然 后 设 定 
解 算 硕 。 求 解 算法 选择 龙 格 - 库 塔 数值 解法 ， 设 定 模 型 仿真 步 长 为 0.001 s. 
输入 端口 包括 车 辆 纵向 速度 和 车 轮转 角 ， 输 出 端口 包括 车 辆 位 置信 息 、 纵 横向 
速度 等 运动 学 信息 ， 以 及 横 摆 角 、 质 心 侧 偏 角 等 动力 学 信息 。 完 成 上 述 设 定 
后 ， 通 过 外 部 接口 将 车 辆 模型 发 送 至 指定 路 径 下 的 Simulink 仿真 文件 中 。Car- 
Sim 模块 即 以 S 頁 数 的 形式 増加 色 Simulink 模型 库 中 。 通 过 调用 该 S 函数， 并 
加 入 控制 器 模块 和 外 部 输入 信息 ， 即 完成 联合 仿真 环境 的 搭建 ， 如 图 4.7 所 示 。 





Reference 





Messurements 
MPCController 







CarSim S-Function 


Terminator Vehicle Code:i i 





Terminator] 


图 4.7 Simulink/CarSim 联合 仿真 平台 


4.4 仿真 实例 


本 节 实例 主要 讲解 如 何 通过 搭建 Simulink/CarSim 联合 仿真 平台 ， 对 所 设 
计 的 控制 器 进行 仿真 验证 ， 通 过 实践 加 深 对 理论 的 理解 。 图 4. 8 是 对 基于 MPC 
的 给 定 轨 迹 的 轨迹 跟踪 控制 器 进行 CarSim 与 Simulink 联合 仿真 的 实例 

选用 CarSim 作为 车 辆 动力 学 仿真 软件 ， 版 本 为 8. 02。 需 要 注意 的 是 ， 选 用 
的 控制 系统 仿 直 软 件 Matlab/Simulink 版 本 不 能 过 高 ， 否 则 CarSim 模块 无 法 通过 
外 部 接口 增加 到 Simulink 模块 库 中 。 高 版 本 的 Matlab 如 何 与 CarSim 联合 仿 直 ， 
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[me 上 


Steer L. 







CarSim S-Function 
Vehicle Code: à 









Constant 
U Control 


To Workspacel Unit Delay 


[$4.8 Simulink/CarSim 联合 仿真 平台 
有 特殊 的 设置 方法 ， 详 见 附 A。 本 文选 用 的 Matlab/Simulink 版 本 为 2011a。 
4.4.1 CarSim 5 Simulink 联合 仿真 


(D 运行 CarSim 。 这 里 选用 的 CarSim 的 版 本 为 8. 02. 

② 出現 “Select Recent Database". (“选择 数据 库 ”) 对 话 框 ， 如 图 4.9 所 
示 ; 选择 “D: \ Users V Public V Documents V CarSim. Data” 数 据 座 文件 ( 注 
意 数据 库 文件 的 位 置 与 自己 安装 过 程 有 关 )， 点 击 “ Continue. with the selected 


database" 。 


zB Select Recent Database 








Recently used database folders 


|D:XUsersVPubliciDocuments|CarSim Data 
C-\Users\ Public\Documenta\CarSim Data QS 
C:\Users\Public\Documenta\CarSim Data\Data 
C:\Users\Public\Documents\CarSim Data 





Continue with the selected database “Remove selected folder from list ] 

















Browse for another database... | Create new database from a consolidated parsfile.. | | 


Don't show this window the next time you start ] 











图 4.9 数据 库 选择 
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③) HI “License Settings”(“ 许 可 设置 ") 对 话 框 ， 如 图 4. 10 所 示 ; 选择 
“Select”， 即 可 打开 CarSim 的 主 界面 ， 如 图 4. 11 所 示 。 


rọ? License Settings 



















Check Days to Tnel 

? Product name Fealurecode — HostlD KeylD Version expiration license? 
-af3m Simularen Graphic Userlntertace (SGU) carsimsguCf Teem SolidSOUAD 2010 — 000001 9.00 32768 
C tor Windows cstsimCN Team SohdSOUAD 2010 *0000C1 $ 00 32768 
n Trailer tor Windcws cersimtCN Team SohdSOLlAD 2010 V 000001 900 32768 
Sensors carsimsmoCN Team SolidSQUAD 2010  K000001 300 32768 
T «ath Sensors carsimsmortCN Teem SolidSQUAD 2010 — K000001 9.00 32768 
xs siver tor Opal-RT CorsimopelCN Team SolidSOUAD 2010 K.U0UUD! 9 UU 32768 
Trailer tor Opsi-RT carsimcpelrzN Team SolidSQUAD 2010 k000061 300 32768 
Solver for Lirux FT corsimlinux«CH Teom SohdSOUAD 2010 K0000C1 53.00 32768 
Trailer tor rux-RT carsimlinuxtCN.— Team SohdSOUAD 2010 K000001 900 32768 
Soter for d' CE-RT carsImdspace( Team SolidSOUAD 2010 K000001 300 32768 
iler tor dS PACE-FT carsimdspacen Team SolidSOUAD 2010 K 000061 200 32768 
に ョ Sim Solver for ADI-RT camimadiCN Team SolidSOUAD 2010 ko00001 9.00 32768 
Trailer for ADIKET carsimadilCH Team SolidSOLlAD 2010 K000001 900 32768 
> Solver tar Lobmew RT carsimlebraewC Team SolidSOUAD 2010 «000001 300 2758 
z^rGim Trailer tor Labview- RT carsimlebaewt! Team SolidSOUAD 2010 — K000061 900 32768 
afSim V. es Pin Simulator cmsirmdsCN Team SnhdSOUAaN 2010 Knnnnn1 snn 3?7hR 
Windows rtang Simulator Trarer carsimwdstCN Team SokdSOUAD 2010 K000001 300 32768 
carsi CN 32768 K000001 900 32768 
Sm FT with Frame Twist carsimir fi Team SolidSOUAD 2010 ko00001 9 06 32768 
~ (Sm Solver tar ETAS Lat Cat RT csisimlabcaiC「 Team GolidSOUAD 2010 k 000001 900 32760 


Humber of Lae Animatcrs 





Quit Program 


an! show hs window w^en stemng 


图 4.10 许可 设置 
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| 图 4.11 Carsim 主 界面 


CarSim 主 界面 主要 由 3 大 部 分 组 成 : 车 辆 参数 及 仿真 工 况 的 设置 、 数 学 模 
型 求解 与 后 处 理 ， 用 户 可 以 修改 车 辆 参数 ， 根 据 需 要 来 设置 仿真 工 况 ， 
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④ gd; "New", JE i£— tH Dataset, ün[* 4.12 所 示 ; 在 两 个 文本 框 中 分 
别 输入 “Example” 和 “MPCtestL”， 然 后 点 击 “set”， 完 成 新 建 。 


Category Example [x) [TejMPaestl 


The current category and title for this data set are shown in the window title bar The screen will ja Set 四 


| [Nochange 
be locked except for the controls in this panel until you click e button (Se! or No Change) | | No Cheng | 





图 4.12 新 建 Dataset 


© 选择 主 菜 单 中 的 “Datasets” 下 拉 菜 单 (如 图 4.13 所 示 ) ， 会 发 现 Ex- 
ample 一 栏 中 多 出 “MPCTest1”。 


Example - ABS 

External Powertrain ry MPCtest1 
MPC Dynamic 
newABS 








图 4.13 第 4 步 中 新 建 的 Dataset 


u (6 点 击 图 4.14 (a) 中 的 下 三 角 ， 选 择 相应 的 车 型 。 选 择 CS B - Class 中 
288 i Hatchback 车 型 ， 如 图 4. 14 所 示 。 





Test Specifications 
Vehicle conhigurahion. Ind.ind X 
RES 中 | 
(a) 

CS B-Class M P v B-Class, Hatchback 
CS C-Class ^ B-Class, Hatchback: No ABS 
CS D-Class ^ B-Class, Hatchback: Twist Beam 
CS E-Class ^ B-Class, Sports Car 
CS European Van ^ B-Class, Sports Car (Driver's View) 
CS F-Class ^ B-Class, Sports Car (Hard Top) 
cs F3 * B-Class, Sports Car (No Tire Audio) 
CS GT L4 


B-Class, Sports Car w/ Chassis Flex 
(b) 


图 4.14 选择 车 型 


D 新 建 满足 要 求 的 仿真 工 况 。 

点 击 如 图 4. 15 所 示 新 建 工 况 ， 即 出 现 如 图 4. 16 所 示 的 界面 。 

O 新 建 仿真 工 况 ， 点 击 “New”， 在 图 4.17 中 的 文本 框 中 依次 输入 “MPC 

Example” 和 “newSplit Mu", Ji "Set", 
@ 根据 要 求 设置 仿真 工 况 : 

图 4-15 仿真 工 况 > 目标 车 速 为 18 km/h; 
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图 4.16 仿真 工 况 的 主 界面 


Category MPC Example | 了 | “Tile jnewSplt Mu 


The current category and title for this data set are shown in the window title bar The screen will 
be locked e«cepttor Ihe controls in this panel until you click & button (Set or No Change) 





Set | No Change 


li 





图 4.17 新 建 仿真 工 况 


> 无 制 动 ; 

> 挡 位 控制 选用 闭环 AT 6 挡 模 式 ; Driver Controls 

> A4; 

> 路 面 选 择 为 1 km 摩擦 系数 为 1.0 的 
方形 路 面 。 

> 完成 后 的 设置 ， 如 图 4. 18 所 示 。 

D 设 置 仿真 时 间 。 

Ic FE dk E TED; EC T nf dz Veg, d AN 
图 4.19 所 示 的 文本 框 内 输入 “30”。 

fe) 选择 前 面 新 建 的 仿真 工 况 ， 

点 击 “Home” 图 标 ， 返 回 CarSim 的 主 
界面 ， 选 择 前 面 新 建 的 “Split Mu” 工 况 ， 

O 设置 仿真 步 长 ， 




















在 CarSim 主 界面 中 ,点 击 下 拉 菜 单 » " 
“Tools”, jk ff “ Preference”, 出現 Atl 图 4.18 furi cott 


5rjs.cn 000000 


- 


I I 


无 人 驾驶 车 辆 模型 预测 控制 
he BR dal a 
4. 20 所 示 的 界面 ， 将 仿真 步 长 设置 为 “0.001”。 
建立 CarSim 与 Simulink 联合 仿真 的 模型 。 
点 击 如 图 4. 21 所 示 的 Models 选项 ， 选 择 “ Models: Simulink”。 








Start and Stop Conditions Default Settings for Math Models 
- Default time step intervals for the math models end output files. used if no other 
ec l values ere provided for a run 
Time (sec) T Road stetion (m) Time step (sec) Freq (Hz) 
Set| — 0 | 0 | Mathmodel| — 000 — (| 1000 
Stop: | 30 [Rioad forward 习 Outputfile:| 0025 | 40 
图 4. 19 仿真 时 间 设 置 图 4.20 设置 仿真 步 长 
Run Control with Simulink 
Run Now | Send to Simulink os 7] x 
， INo linked library] 





| Models: Self-Contained Solvers 
[Mode simui 
、 Models: Transfer to Local Windows Directory 

Models: Transfer to RT-Lab Target 

Models: Transfer to dSPACE Target 

Models: Transfer to Target by FTP 

Models: ETAS ASCET 

Models: LabVIEW for Windows (VI) 

Models: Transfer to LabVIEW RT Target 








图 4.21 选择 Simulink 接口 


D 点 击 如 图 4.22 所 示 的 选项 ， 选 择 “[ Link to New Dataset ]" 。 

© 此 时 将 弹出 一 个 如 图 4. 23 所 示 的 对 话 框 ， 在 对 话 框 中 依次 输入 “Ex- 
ample" 和 " MPCtestl ", ;il; "Create and Link” 完 成 新 建 ， 如 图 4.24 
所 示 。 


Run Control: Built-In Solvers 




















v Link to New Dataset mmi 
Category tor new dataset 
| Example LY 
ti Title for new dataset 
Active Suspension MPCtest1 
Brake Cont. with Moving Objects (2 Sensors) - Am 
Brake Cont. with Moving Objects (CS8) | Create and Link | | Cancel | 
Brake Control with Moving Object 
图 4.22 Link to New Dataset [4.23 新 建 一 个 与 Simulink 联合 的 Dataset 


@ 点 击 如 图 4.25 所 示 的 “MPCtest1”， 将 会 弹出 如 图 4. 26 所 示 的 Simu- 
link | Example} MPCtestl 的 主 界面 。 如 图 4.27 所 示 ， 选择 相关 路 径 ， 
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. Run Control with Simulink 








(Bun Now | Send to Simulink "^s - v) 





[No Dataset) 

[Copy and Link Dataset) 

ABS Controller 

Active Suspension 

Brake Cont. with Moving Objects (2 Sensors) 
Brake Cont. with Moving Objects (CS8) 
Brake Control with Moving Object 
Custom Steer S-Function 

Driver Model (CS8) 

External Springs 

Four Wheel Steering System 

Hitch Controller 

Type A ABS Controller 

Type 8 ABS Controller 





Adaptive cruise control ld 
ESC , 
| Example ple と = HET a ABS 
External Powertrain * v MPCtest1 
newABS 


[44.24 选择 新 建 的 “MPCtestl ” dataset 


> 浏览 工作 路 径 : D; \ Users | 
Run Control with Simulink 
UN C . 5 "arSi "E 4 
Public V Documents V CarSim_ Data ーーー ーー デー 


> Simulink Model 的 K $$: D; \ p— 1 


Users V Publice V Documents V CarSim Data ^ 
Extensions V Simulink V MPCtestl. mdl. 





图 4.25 Ail "MPCtestl " 


览 MPCtestl. mdl， 如 几 4. 27 所 示 。 

(e) 定义 CarSim 的 导入 变量 

点 击 如 图 4.28 (a) Br "Import Channels”, PE “1/0 Channels; Im- 
pot"; 然后 点 击 如 图 428 (b) 所 示 的 “[ Copy and Link Dataset ]" ; 
如 图 4.28 (c) 所 示 ， 输入 “MPCtest] input" 

jid; "MPCtestl input” 图 标 ， 显 示 如 图 4. 29 所 示 界 面 。 这 里 需要 浏览 找到 
“Readme file for imports ”图 标 ， 其 后 的 文本 框 里 显示 “Programs \ solvers \ 
ReadMe \ 人 i. i s imports tab. txt” JEX CarSim 的 导入 变量 为 车 速 和 4 个 
车 纶 的 转角 ， 顺 序 依 次 为 IMP_ SPEED (质心 车 速 [km/h]), IMP. STEER_ LI 
(左前 轮转 角 [°]), IMP_ STEER_ RI (右前 轮转 角 [°]), IMP_ STEER_ 12 
( 左 后 轮转 角 [2])、 以 及 IMP_ SIEER_ R2 ( 右 后 轮转 角 ["])， 如 图 4.30 所 示 。 
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BS X $B? 


Sidebar Refresh Hep Loci 








[0 Wren NP DocuneniCaSm Du? E Sdirk Model |D Uer Public Document Ca Sem, DaleEmererra\Smui MP — 


You can wate rotes here lor tha 1 de Live ainsa rebesh e | Mz Faft Wreden 715 [le IDEM マ 
ueee [E Mderthy Sm wor no chectory - t nion Ae 
Ese ivo ep fore ‘Use the field below for data ox commands assocaled mth iht modal data tet 


Irleg don method [AN-2 (2 updates pm step] v] | 
F) Smirk time dep = 1/2 CaSm tme step | 
Muzelanecus setting: lor advanced uters 


w^ 








图 4.26 Simulink | Example; MPCtestl 的 主 界面 





(D'UserrPublADocumertsiCerSm Delo — de) Simulnk Model. D XUsentPutliciDocumentiCerSies, DetsiÉxtensionsiS mini MP eM ml 
Weing ureco i 3 Live animator refresh rate: — H: Fa baut Windows CS he hatat - 
IV jdenty Simuink working dactoryl 

E Speoty atemehve VS solver DLL files 
F) Settime step here 


图 4.27 选择 路 径 


_ . impotChannels > - Iv 
[No linked library] 
External PARSFILE 
Generic Data Group 
Generic Data Lists 
Generic V$ Commands 

Y |/O Channels: Import 

















Driver Import 
Hitch Controller 
Y MPCtest1 input 


(b) 
[84.28 新 建 MPCtestl input 
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«v Link to New Dataset 







Category for new dataset 





Title for new date: 
MPCtest1 inpul 





(c) 
[54.28 新 建 MPCtestl input. ( ££) 


— 
















4$ VO Channels: Import; MPCtestl input ーー 

Bie Ed Qatasets Libraries Tools View Help Ey a U ER 
> トマ "4 g 1 Ch127 

< T 会 4 と Ha B Gy - p -) 02-16-2014 20 42 48 X * を 

Back vc Mame Prevous hert New Sme ne Sede LpTool Parsfie Detre Sidebar Refresh Heip Lock 

Mes [Tuani Bara. i Resda fie lor mpors Prom sclven Fesdáe V ii s moots lab bd [J heh) [Vies Speadsheel ) 


fou can vate rotes here Lor he 


aiaia ~ Eategoiews č Venable: Available for import Active Import Variables č  ž < 
U——n t) “| Move Nome Mode intai Vue * 
| . ieleched SPEED 
1 IMP SFEED 18 
| pwn — < IMP, STEER LI Adi 00 
mlhe ket 5 


















3 IMP STFER f Add 
合 4 IMP STEER は Ada 00 
5 IMP. STEER, R2 - t Add o0 














Dobie ckcs to acyale a vanable 
Ast ed rat vapabler con be dele ed unc Qi ogee De dc a rove rmribm lo deactivate a vi 


egim ha you provide hese The rias 
£f, IN «nnne « rme 

mese onmes i he name of Ihe acie import vaatia 
anc ceqpresinno is ror extietacn involving 


malh model yarahet Diha VebuleC commandi: can be 
Mte to deire makay vanabies 


图 4.29 定义 CarSim 导 人 变量 


需要 注意 的 是 : CarSim 的 导入 变量 和 Simulink 中 MPC 模型 的 输出 量 是 相 


对 应 的 ， 所 以 这 里 的 CarSim 的 导 人 变量 应 按照 如 图 4. 30 的 顺序 排列 选择 。 


Active Import Variables 


Name Mode Initial Value 






Replace | 
Add — 





1 

2 MP STEER L1 
3 IMP_STEER RI č 
1 

5 


IMP STEER. L2 
IMP. STEER. R2 











[44.30 — CarSim HJ FAE ht 
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(D 定义 CarSim 的 导出 变量 。 

步骤 同 @ 定 义 CarSim 导入 变量 相同 。 首 先 新 建 名 为 “MPCtestl output" 
的 一 个 dataset。 点 击 “MPCtestl output". 图 标 ， 弹 出 如 图 4.31 所 示 的 界面 。 
这 里 需要 浏览 找到 “Readme file for outputs” 图 标 ， 其 后 的 文本 框 内 显示 为 
“ Programs V solvers V ReadMe V f_ i_ outputs tab. txt” 定义 CarSim 的 导出 
变量 依次 为 X0 (坐标 系 X 轴 的 坐标 值 [m]), YO (坐标 系 Y 轴 的 坐标 值 
[ m] ) , Yaw ( 偏 航 角 [°])，Vx (质心 处 的 纵向 车 速 [km/h]), 以 及 Steer_ 
SW (方向 盘 转角 [°])。 








$ 只 会 る すす B m レー D) emen X gw 


Save Umb ese tbTool Parsi 





- [Fedme lie for apu | Prog ami ircher RescMev ! Lodpu abb 


入 Categories = Available Venables n Show Long Hanes. 
[13 — - 








































E i — Doubie-ckck on a vanable name here to actae t Double-click on a vanable rame here to dasctvste ë 
ot canbe uang equeliorm 
thal you provide here. The syriar ir Opbonal equation: (writen and processed alter the above kit] 


DEFINE, OUTPUT «rame» = expression - cundi 







„| where cameo i the name ol ihe naw odi pul variable 








图 4.31 定义 CarSim 导出 变量 


需要 注意 的 是 : CarSim 的 导出 变量 (XO, YO 和 Yaw) 和 Simulink 中 MPC 
模型 的 输入 量 是 相对 应 的 ，CarSim 的 导出 变量 (Vx 和 Steer. SW) 为 观测 量 、 
所 以 这 里 的 CarSim 的 导出 变量 应 按照 如 图 4. 32 的 顺序 排列 选择 。 


Active Variables for Export 





图 4.32 CarSim 的 导出 变量 
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( 点 击 “Home” 图 标 ， 返 回 CarSim SI 
: e Run Control with Simulink 
的 主 界面 。 点 击 如 图 4.33 所 示 的 “Send to [Run Now [Send to Simulink c: 
Simulink” 图标， 此 时 Matlab 及 之 前 新 建 的 vum POles 
空白 模型 MPCtestl. mdl 将 被 打开 ， 如 ERES z 
[8] 4.34 所 示 。 

D 如 图 4.35 所 示 、 在 “Matlab Com- 图 4.33 数学 模型 求解 器 
mand Window” 中 输入 “Simulink”， 即 可 打开 Simulink 库 浏 览 窗 口 。 注 意 现在 
的 Simulink 库 浏 览 窗 口 比 单独 运算 Matlab/Simulink 时 多 了 一 个 “CarSim S - 
Function” 图 标 。 如 图 4.36 所 示 , 将 “CarSim S - Function” 图 标 拖 电 到 MPC 
testl. mdl 中 ， 生 成 的 新 模块 恰好 有 一 个 输入 接口 和 一 个 输出 接口 ， 分 别 对 应 
着 CarSim 的 导入 变量 和 导出 变量 。 









Wi MPCtest1 


File Edit View Simulation Format Tools Help 


Dic i «55€ |!!!» 














[5| 4. 34 MPCtestl. mdl 


4& MATLAB Command Window il 


To get started, type one of these: helpuin, helpdesk, or demo. 
For product information, visit www.mathworks.con. 


> Warning: Calling MEX-file ‘D:\Program FilesNCarSin802 ProgVProgransNSoluersNos SF.dll'. 
X-files with .dll extensions will not execute in a future version of MATLAB. 





[44.35 Matlab Command. Window 命令 窗口 
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] W| Simulink Library Browser seas [stes 

















CarSim S- 
Function 









N.. CarSim S-Function 

由 -天 Communications Blockset 
に E Control System Toolbox 
i- I Data Acquisition Toolbox 
由 .而 EDA Simulator Link 

&- M Embedded DE Link 

&- M Fuzzy Logic Toolbox 


[$4.36 含有 CarSim S 函数 模块 的 Simulink Library Browser 


完成 上 述 设 定 后 ，CarSim 通过 外 部 接口 将 车 辆 模型 发 送 至 指定 路 径 下 的 
Simulink 仿真 文件 中 ，CarSim 模块 即 以 5 函数 的 形式 增加 到 Simulink 模型 库 中 。 
① 在 MPCtestl. mdl 中 加 入 基于 MPC 的 轨迹 跟踪 控制 器 ，CarSim 的 导出 量 
经 过 该 控制 器 的 计算 ， 决策 出 下 一 时 刻 的 质心 车 速 和 前 轮 偏 角 ， 然 后 导入 到 


CarSim 模块 里 。 


将 Simulink 浏览 窗口 中 的 “S - Function” 图 标 拖 电 到 MPCtestl. mdl 中 ， 
显示 新 的 5 函数 模块 。 双 击 该 图 标 ， 弹 出 如 图 4.37 所 示 的 对 话 框 ， 在 “S - 


L 


Wi Function Block Parameters: S-Function1 es 

















S-Function 


User-definable block. Blocks can be written in C, M 
(level-1), and Fortran and must conform to S-function 
standards. The variables t, x, u, and flag are 
automatically passed to the S-function by Simulink. You 
can specify additional parameters in the 'S-function 
parameters’ field. If the S-function block requires 
additional source files for the Real-Time Workshop build 
process, specify the filenames in the 'S-function 
modules' field. Enter the filenanes only; do not use 
extensions or full pathnames, e.g., enter 'src srcl', not 
'src.c srcl.c'. 









Parameters 
S-function name: | WY WPCCont rol le r: 3 | 


S-function parameters: 






S-function modules: '' - 


-—9- | py 


图 4.37 S 函数 对 话 框 
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NEN D E 
Function name” 中 输入 “MY_ MPCController3" 。 点 吉 "Edi", Ji üfüy 
T MPC 的 轨迹 跟踪 控制 算法 的 m 文件 导入 ， 显 示 为 “MY_ MPCController3 " 
的 5 图 数 图 标 。 注 意 此 模块 恰好 有 一 个 输入 接口 和 一 个 输出 接口 ， 分 别 对 应 着 
轨迹 跟踪 控制 器 (MY. MPCControllers) 的 导入 变量 和 导出 变量 。 

其 中 ，MY_ MPCController3 控制 器 模块 的 输入 量 为 上 一 时 刻 CarSim 模块 
的 导出 变量 XO, YO 和 Yaw. 在 如 图 4. 38 所 示 的 Simulink/CarSim 联合 仿真 平 


台中 加 入 延 时 模块 。 
La 
RIO Li 
: O 
Laci 


4 ced 
Yo 
| Xo o Workspace Scope 



















CarSim S-Function 
Vehicle Code:i i 






Constant 
U Control 








To Workspacel Unit Delay 


[44.38 Simulink/CarSim 联合 仿真 平台 

MY |. MPCController3 控制 需 模 块 的 输出 量 为 车 速 w (m/s) 和 前 轮 偏 角 6 

(deg), ， 震 对 其 转换 成 满足 CarSim 模块 要 求 的 导入 变量 : IMP_ SPEED (质心 

车 速 [km/h]), IMP. STEER_ LI (左前 轮转 角 [^]), IMP_ STEER_ RI 

(右前 轮转 角 [^]), IMP. STEER_ I2 ( 左 后 轮转 角 [°]), UK IMP_ 

STEER_ R2 ( 右 后 轮转 角 [?])。 所 以 , 在 MY_ MPCController3 控制 器 模块 的 
输出 量 与 CarSim 模块 的 导 人 变量 之 间 加 入 了 转换 函数 。 


定义 : 
IMP... SPEED 36 0 
IMP. STEER_ Ll 0 180/pi 
IMP. STEER_ R1|- | 0 180/pi [| (4.24) 
IMP. STEER_ 12 0 0 
IMP. STEER_ R2 0 0 


人 返回 Carsim 主 界面 、 点 市 “Run Now", 运行 结束 后 ， 再 点 击 “Send to 
Simulink”， 弹 出 MPCtestl. mdl， 如 图 4.38 所 示 ， 运 行 整个 模型 
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说 明 : 若 対 CarSim 软件 的 参数 〈 车 辆 参数 、 仿 真 工 况 等 ) 进行 修改 ， 则 
要 再 点 击 “Run Now” ， 运 行 结束 后 ， 青 点 击 “Send to Simulink”, KEHRA 
修改 后 的 参数 ， 最 后 运行 模型 。 若 只 在 Simulink 模型 里 做 相应 的 修改 ,不 会 影 
ifj CarSim 中 的 参数 ， 直 接点 击 模型 。 在 Simulink 中 运行 仿 直 模 型 ， 跟 踪 圆 形 ， 
可 观测 到 轨迹 曲线 ， 如 图 4. 39 所 示 。 





图 4.39 Simulink 中 加 入 控制 器 后 的 轨迹 曲线 


do 观察 加 入 基于 MPC 的 轨迹 跟踪 控制 器 后 车 辆 跟踪 圆 形 轨迹 ， 点 击 
“Animate”， 查 看 动画 ， 如 图 4. 40 所 示 ; 同样 可 点 击 “Plot” 绘 制 曲线 





图 4.40 仿真 动画 
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4.4.2 基于 MPC 的 轨迹 跟踪 控制 器 的 设计 


本 节 主 要 介绍 如 何 通 过 Simulink 的 S 函数 设计 基于 MPC 的 轨迹 跟踪 控制 
üt. S PARON Simulink 中 的 一 个 模块 。 其 运算 过 程 符合 Simulink 模块 的 仿真 过 
程 。S 了 苑 数 格式 非常 严格 ,在 Simulink 中 有 一 个 模板 m 文件 ， 使 用 时 可 以 在 此 
模板 上 简单 修改 。 此 模板 m 文件 由 主 函 数 以 及 子 函 数 (不同 flag 值 调用 的 函 
数 ) 组 成 。 该 m 文件 可 直接 调用 生成 MY | MPCController3 控制 器 的 S 函数 。 
该 程序 详 见 chapter4_ 4_ 3. m 文件 。 

为 便于 代码 解析 ， 将 仿真 程序 分 为 若干 模块 分 别 介 绍 ， 程 序 后 面 有 备注 ， 
方便 读者 阅读 

第 一 部 分 妨 主 員数 : 





function[sys,xO0,str,ts]-MY MPCController3 (t,x,u,flag) 
switch flag, 

case 0 名 初始 化 

ys,x0,str,ts]-mdlInitializeSizes; も Initialization 
case 2 更 新 离散 状态 量 

sys =mdlUpdates (t,x,u); % Update discrete states 
case3 当 计 算 输 出 

sys -mdlOutputs (t,x,u);$ Calculate outputs 

case {1,4,9} $ Unused flags 

sys-[]; 

otherwise 未 知 的 flag 值 

error(['/unhandled flag-;num2str(flag)]); $ Error handling 
end 

o os KAET R 

di D E 





function[sys,x0,str,ts]-mdlInitializeSizes 


sizes -simsizes; 


sizes.NumContStates -0; £ 连续 状态 最 的 个 数 
sizes.NumDiscStates =3; $1 Z HACK S 最 的 个 数 
sizes.NumOutputs =2: % 输出 時 的 條 数 
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一 一 一 一 全 
sizes.NumInputs =3: $ 输入 量 的 个 数 
sizes.DirFeedthrough-1; % Matrix D is non-empty. 直接 贯 

通 标示 

sizes.NumSampleTimes =1; 采样 时 间 的 个 数 
sys =simsizes (sizes); 
x0 =[0 ;0 ;0 ]; 状态 量 初始 化 
global U; 
U=[0 ;0] ; 
Str]; $ Set str to an empty matrix. 
ts as[0,.05 0]; $ sample time:[period,offset] 


gs 初始 化 子 函 数 结束 
第 三 部 分 为 更 新 离散 状态 量子 函数 : 





function sys =mdlUpdates (t,x,u) 
SyS =X} 


% End of mdlUpdate. 
第 四 部 分 为 计算 输出 子 函 数 ， 是 轨迹 跟踪 控制 的 主体 。 在 程序 解析 时 我 们 


会 将 第 二 节 的 理论 算法 与 代码 对 应 ,方便 读者 的 理解 。 


oe 





function sys -mdlOutputs (t,x,u) 


global a b u, piao; 


global U; 
global kesi; 
NX z3; gs 状态 量 的 个 数 


Nu -2; S 控制 量 的 个 数 

Np=60:  $ 预测 步 长 

Nc-30;  $ 控制 步 长 

Row=10; $ 松弛 因子 

fprintf ( “Update start,t=%6.3f\n’,t) 

t_d =u (3) *3.1415926 /180;% CarSim 输出 的 为 角度 ,角度 转换 为 弧度 
期 望 轨迹 为 圆 形 轨迹 的 代码 ,读者 可 根据 实际 情况 设计 所 需 路 径 方程 ， 

S 源 代 码 文 件 中 有 直线 轨迹 代码 

* 半径 为 25 m 的 圆 形 轨迹 ,速度 为 5mA 
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r(1-225*sin(0.2*t); 
r(2225 +10 -25 * cos (0.2 *t); 
Ér(3).0.2 t; 
valzs5; 
vd2 20.104; 
を 半径 为 25m 的 圆 形 轨迹 ,速度 为 3m/s 
r(1-225*sin(0.12*t); 
r (2)=25 «10-25 * cos (0.12 *t); 
r(320.12*t; 
Yadls3s 
vd2=0.104 ; 
半径 为 25m 的 圆 形 轨迹 ,速度 为 10m/s 
r(1225*sin(0.4:5t); 
r (2)=25+10-25 * cos (0.4 st); 
r(320.4*t; 
vdisi10; 
vd2 20.104; 
gs 半径 为 25m f UE BL , SERE JJ 4m /s 
r(1225*sin(0.16*t); 
r (2)=25+10 -25 * cos (0.16 t); 
r(320.16*t; 
Vd1 =4 : 
vd2-20.104; 


る 参数 设计 


0 de Æ ge o? oe oe oe 


o0 o9 o0 o? o? o? mm oe coe 


op 


kesi= zeros (Nx -Nu,1); 

kesi (1)=u(1)-r(1): uu(1)==X (1) 

kesi (2)=u (2)- r (2); $% u(2)==X (2) 

kesi (3)=t_d-r(3); % u(3)==X (3) 

kesi (4)=U (1); 

kesi (5)=U (2); 

fprintf (‘Update start,u(1)- $ 4.2f\n ',U (1)) 
fprintf('Update start,u(2)- $ 4.2f\n ',U (2)) 
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Ta, 05; 

T all-40; % 总 的 仿真 时 间 , 主 要 功能 是 防止 计算 期 望 轨迹 越界 
$ Mobile Robot/ Vehicle Parameters 

Lz2.6; 

$ Mobile Robot/ Vehicle variable 


u, piao = zeros (Nx,Nu); 
Q- eye (Nx * Np,Nx * Np); 
R =5 * eye (Nu *NcC); 
a= [1 0 -vdl*sin(td)*T; 
0 1 vdl*cos(t d)*T; 
0 0 Isl: 
bs[cos (t d)«'T 03; 
sin(t d)*T 0; 
tan (vd2 ) * T/L vdl * T/(cos (vd2 ) ^2 );]; 
S 以 上 代码 对 应 式 (4 .6 ) 中 的 参数 
A_cell =ce11] (2,2); 
B ce8elzdcell(2,D; 
A cellí(1,1)-a; 
A cell(1,2)-b; 
A cell(2,1)-zeros (Nu,NXx); 
A cell(2,2)-eye (Nu); 
B cell(1,1)-b; 
B cell(2,1)-eye (Nu); 
A-cell2mat (A cell); 
B-cell2met (B cell); 
Cc-2[10000;01000;001 0 0;]; 
gs 以 上 对 应 式 (4 .10) 中 的 参数 
PHI cell-cell (Np,1) : 
THETA cell-cell (Np , Nc) : 
for4se1l:1:Np 

PHI, cell(j,1)»2C*A^j; 
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for k =1 :1 :Nc 
Tf eec) 
THETA cell(j,k)2C* A^(j- k) * B; 
else 
THETA, cell(j,k)- zeros (Nx,Nu); 
end 

end 
end 
PHI -cell2mat (PHI, cell); $ S1ze (PHT)=[Nx * Np Nx+ Nu] 
THETA = cell2mat (THETA cell); $ size (THETA )=[Nx * Np Nu * 

(Nc+ 1 ) ] 

S 以 上 对 应 式 (4 .12) 中 的 参 数 
H_ce]] =cell (2,2); 
H_ce11 (1 ,1)- THETA” * Q* THETA +R; 
H cellí(1,2)- zeros (Nu * Nc ,1) ; 
H cell(2,1)-zeros (1,Nu * Nc); 
H cell(2,2)- Row; 
H=cell2mat (H cell); 
error - PHI * kesi; 
f cell-cell(1,2); 
f cell(1,1)22 *error' * Q:* THETA; 
f cellí1,2)-0; 
f-cell2mat(f cell); 
S 以 上 对 应 式 (4 .19) 中 的 参数 


A t - zeros (NC,Nc) ; 
for p-21:1:Nc 
for qsl1:i:Nc 


ifq«-p 

A t (p,d)-1; 
else 

A t (p,q)-0; 
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end 

A I-kron(A t,eye(Nu));$ 对 应 式 (4.17) 中 的 参数 

Ut = kron (ones (Nc ,1) ,U) ; 

umin-[-0.2; -0.54;];$ 维 数 与 控制 变量 的 个 数 相同 

umax -2[0.2 ;0.332]; 

delta umin-[-0.05; -0.0082;]; 

delta umax-[0.05;0.0082]; 

Umin = kron (ones (Nc,1),umin); 

Umax - kron (ones (Nc,1),umax); 

A cons cell- (A I zeros(Nu* Nc,1); -A_I zeros (Nu * Nc,1)); 
b. cons. cell-(Umax -Ut; -Umin +Ut}; 

A cons = cell2mat (A cons. cell); 

gs (求解 方程 ) 状 态 量 不 等 式 约束 增益 矩阵 ,转换 为 绝对 值 的 取 值 范围 
b. cons = cell2mat (b cons, cell); 


$ (求解 方程 ) 状 态 量 不 等 式 约束 的 取 值 


M=10: 
delta, Umin- kron (ones (Nc ,1) , de1ta_umin ) 
delta Umax- kron (ones (Nc,1),delta umax); 


1lb-[delta Umin;0];$ (求解 方程 ) 状 态 量 下 界 , 包 含 控 制 时 域内 控制 增 


量 和 松弛 因子 
ub -[delta, Umax;M] ;* (求解 方程 ) 状 态 量 上 界 , 包 含 控 制 时 域内 控制 增 
量 和 松弛 因子 
E ニニ ニニ ニニ ニニ ニー ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ 
o 开始 求解 过 程 
% ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニー 


options = optimset ('Algorithm;áctive -set ); 

る options -optimset('/Algorithm;interior -point - convex ); 

[X,fval,exitflag]-quadprog (H,f,A, cons,b. cons, [], [],1b, 
ub, [],options); 
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s 计算 输出 
u_p1ao(1 )=X (1); 
u_ pao (2 )=X (2); 
)= kesi (4)+u_ piao (1) : 用 于 存储 上 一 个 时 刻 的 控制 量 
U(2)= kesi (5)+u_ piao (2); 
u_real (1)- U (1)+vd1; 
u_real (2)=U (2)+vd2 : 
sys =u_real; 
toe 


% End of mdlOutputs. 


4.5 基于 运动 学 模型 的 轨迹 跟 蹊 仿真 结果 分 析 


为 了 验证 所 设计 的 轨迹 跟踪 控制 器 ， 在 不 同 的 轨迹 下 对 其 跟踪 能 力 进 行 仿 105 
直 测 试 。 基 于 运动 学 模型 的 轨迹 跟踪 控制 器 主要 用 于 泊 车 等 低速 工 况 。 根 据 其 
运动 特点 选取 直线 轨迹 和 圆 形 轨迹 进行 跟踪 。 

(1) 直接 轨迹 跟 踊 

分 别 在 参考 速度 为 3 m/s, 5 m/s 和 10 m/s， 参 考 前 轮 偏 角 为 0 的 情况 下 
进行 仿真 测试 ， 直 线 轨迹 以 参数 方程 的 形式 给 出 : 


x(t) = val 
y(t) 25 (4.25) 
e(t) 20 


P, oa 为 期 望 的 纵向 速度 

设置 初始 时 刻 为 mw =0， KAHTA mm 250 s， 初 始 状 态 设置 为 : xo = 
x C) 20, y, 2y(4) =0, po =pl) 20. 约束 条 件 采 用 控制 量 约束 和 控制 
增 量 约束 ， 设 置 与 式 (4.18) 保持 一 致 。 模 型 预测 控制 器 的 基本 参数 设置 
为 :预测 时 域 N, =60， 控 制 时 域 W、=30、 控 制 周期 7=0.05 s, 

仿 直 结果 如 图 4. 41. [44.42 和 图 4. 43 所 示 。 其 中 ,图 4. 41 (a) 为 车 
辆 跟踪 直线 轨迹 的 结果 ， 图 4.41 Cb) 为 对 应 的 轨迹 跟踪 偏差 。 从 图 4.41 
中 可 以 看 出 ， 在 有 具备 初始 储 差 的 情况 下 ，3 种 不 同 参考 速度 下 无 人 驾驶 车 辆 
都 能 迅速 跟踪 直线 轨迹 。 进 一 步 观察 发 现 ，10 m/s 的 情况 下 出 现 轻微 的 超 
调 ， 这 主要 是 因为 在 该 速度 下 使 用 设 定 的 预测 时 域 已 经 不 太 适用 。 通 过 增 大 
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预测 时 域 的 范围 ， 可 以 消除 这 种 现象 。 也 就 是 说 ， 随 着 速度 的 增 大 ， 预 测 时 
域 的 范围 要 求 越 大 。 图 4. 42 和 图 4. 43 为 控制 量 〈 前 轮 偏 角 和 速度 ) 随时 间 
的 变化 曲线 。 可 以 看 出 ， 无 论 是 前 轮 偏 角 ， 还 是 速度 ， 都 能 快速 跟踪 期 望 轨 
迹 并 被 限定 在 约束 范围 内 。 通 过 右 侧 的 局 部 放大 图 能 够 进一步 观察 到 跟踪 过 
程 中 控制 增 量 没有 突变 的 现象 ， 每 个 控制 周期 内 的 控制 增 量 都 保持 在 约束 范 
国内 。 


Y/m 


跟踪 偏差 /m 





X/m 
(a) 





图 4.41 跟踪 直线 轨迹 结果 与 跟踪 偏差 
(a) 参考 轨迹 与 实际 轨迹 ;(b) 跟踪 偏差 


(2) 圆 形 轨迹 跟踪 

圆 形 参 考 轨 迹 依然 以 参数 方程 的 形式 给 出 : 
x(t) = 25sing, 
y(t) = 35 - 25cosg, 


"TS um 
式 中 ，5 为 期 望 前 轮 偏 角 。 

仿真 过 程 中 vv 依次 取 为 3 m/s, 5 m/s 和 10 m/s, 8, 设 定 为 5.94°。 设置 初 
始 时 刻 为 =0， 末 端 时 刻 ts, =50 s， 初 始 状态 设置 为 : xo =x(46) 20, v, = 
y(to) =0。 约束 条 件 和 模型 预测 控制 器 的 基本 参数 设置 与 直线 轨迹 仿 直 保持 
一 致 。 

仿真 结果 如 图 4.44、 图 4.45 和 图 4. 46 所 示 。 其 中 ,图 4.44 (a) 为 车 
辆 跟踪 圆 形 轨迹 时 位 置 的 变化 情况 ,图 4.44 (b) 为 轨迹 跟踪 偏差 。 从 图 4. 44 
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-=-= SS 
SM ーーー $m/s 
ーー 10 m/s 


= 
----5mf/s 5 
ーー 10 m/s 

Bir 4e (a fe £^] i 


前 轮 偏 角 / C» 
Wide ta fa C) 





0 10 20 30 40 50 
时 间 /s 


Ca) 


图 4.42 跟踪 直线 轨迹 的 前 轮 偏 角 
(a) 前 轮 偏 角 随时 间 变 化 ;:(b) 前 轮 偏 角 局 部 放大 





ー-- 速度 釣 東 -速度 约束 


0 10 20 30 40 50 60 6.5 7.0 75 8.0 
時 同 /* 时 间 /s 
(a) (b) 


图 4.43 跟踪 直线 轨迹 的 速度 
(a) 速度 随时 间 变 化 关系 ; (b) 速度 局 部 放大 图 


中 可 以 看 出 ， 无 人 驾 融 车 辆 在 控制 器 的 作用 下 能 快速 跟 足 期 望 委 迹 ， 系 统 跟 路 


误差 最 终 收 化 为 0， 有 具有 良好 的 全 局 稳定 性 。 图 4.45, [d 4. 46 为 跟踪 过 程 中 
控制 量 (前 轮 偏 角 和 纵向 车 速 ) 随时 间 的 变化 。 在 整个 跟踪 过 程 中 ， 控 制 量 
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始终 保持 在 给 定 的 约束 条 件 范围 内 。 从 右 侧 的 局 部 放大 图 可 以 进一步 看 出 ， 控 
制 器 所 给 出 的 控制 增 量 也 被 限定 在 了 设 定 的 范围 内 ， 没 有 出 现任 何 突变 的 现 
象 。 因 此 ， 控 制品 不 仅 实现 了 快速 跟踪 期 望 轨迹 的 功能 ， 也 保证 了 跟 踩 过 程 的 
平稳 性 。 


ーーー 3 m/s 

----5m/s 

ーー |0 m/s 
参考 轨迹 


Y/m 


7 向 跟踪 偏差 /m 





X/m 
(a) 





图 4.44 ”跟踪 圆 形 轨迹 结果 与 跟踪 偏差 
(a) 参考 轨迹 与 实际 轨迹 ;(b) 跟踪 偏差 


时 间 /s 
(b) 





图 4.45 跟踪 圆 形 轨迹 的 速度 
(a) 纵向 速度 随时 间 变 化 ;(b) 纵向 速度 局 部 放大 
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Wi4e tfe / C» 





3.0 3.5 4.0 4.5 5.0 


图 4.46 跟踪 圆 形 轨迹 的 前 轮 偏 角 
(a) 前 轮 偏 角 随时 间 变 化 ;(b) 前 轮 偏 角 局 部 放大 
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上 一 章 详细 介绍 了 基于 车 辆 运动 学 模型 的 无 人 驾驶 车 辆 轨迹 跟踪 方法 ， 并 
通过 搭建 Simulink/CarSim 联合 仿真 平台 对 算法 进行 了 验证 ,初步 掌 握 了 利用 S 
函数 编写 模型 预测 控制 器 以 及 在 CarSim 中 搭建 车 辆 模型 的 方法 ; 但 仅仅 掌握 
这 些 知识 还 不 足以 将 模型 预测 的 方法 真正 应 用 在 无 人 驾驶 车 辆 上 

无 人 驾驶 车 辆 不 同 于 一 般 的 室内 轮 式 机 器 人 。 它 更 多 是 以 较 高 速度 在 复杂 
的 交通 环境 中 行驶 。 为 了 提高 无 人 驾驶 车 辆 在 高 速 行驶 时 的 可 靠 性 ， 有 必要 在 
控制 器 中 引入 车 辆 动力 学 模型 。 以 准确 的 动力 学 模型 作为 预测 模型 ， 可 以 提高 
控制 器 对 车 辆 未 来 行为 的 预 估 能 力 ， 进 而 在 保证 车 辆 稳定 运行 的 同时 ， 充 分 利 
用 车 辆 的 潜能 ; 同时 ， 无 人 驾驶 车 辆 在 高 速 运动 下 执行 机 构 的 控制 输入 ， 轮 胎 
与 地 面 摩擦 引起 的 滑 移 ， 以 及 横向 加 速度 引起 的 侧 倾 等 动力 学 非 线性 约束 条 件 


比 低速 下 更 加 苛刻 。 对 这 些 约 柬 进行 深入 分 析 将 进一步 保障 车 辆 行驶 的 安全 性 
与 稳定 性 。 


本 章 将 以 无 人 驾驶 车 辆 的 主动 转向 控制 为 应 用 背景 ， 建 立 以 车 辆 动力 学 模 
型 为 基础 的 模型 预测 控制 器 。 结 合 车 辆 高 速 运动 下 的 各 类 约束 ， 进 一 步 说 明 复 
杂 约 束 条 件 下 模型 预测 控制 的 求解 方法 。 最 后 ， 依 然 以 Simulink/CarSim 联合 
仿真 平台 验证 算法 的 有 效 性 。 
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© 
5.1 理论 基础 
5.1.1 线性 误差 方程 
考虑 式 (2.38) 所 建立 的 非 线性 动力 学 模型 ， 过 ww =S, (Eus uso H 


H, REE éan ly, i, p, Q, Y, X] RERA Uan =6:， 即 只 对 
车 辆 在 轨迹 跟踪 过 程 中 的 前 轮 偏 角 进 行 控制 ， 而 车 辆 纵向 速度 保持 不 变 。 无 人 
驾驶 车 辆 在 高 速 行驶 时 对 控制 器 实时 性 要 求 更 加 严格 ， 非 线性 模型 预测 控制 难 
以 满足 。 本 曹 继续 采用 线性 时 变 模 型 预测 控制 ， 因 此 首先 需要 对 非 线 性 动力 学 
模型 进行 线性 化 

由 于 期 望 轨迹 无 法 给 出 所 有 状态 点 的 信息 ， 
其 进行 线性 化 ， 得 到 线性 时 变 方程 为 : 


因此 采用 第 3 章 中 的 方式 B 对 

















m Aira D) £u) + B,,(t) u(t) (5. l ) 
y, *ag, 
| df 2C, (25 ーー ニー 
式 中 , Bi (t) = 一 | io Vid X, 2aC,, à 
OU, £ U, "m E m EI 0, I 9 0, 0 
0 
Aal t) = Pirn 
IE as る ,U, 
-2(6,* C.) f, Ü E 2(5C,, -aC.) 00 
m x, dx m x, 
R 2C, Or i əf: 0 あこ 2aC.À,, 00 
m x 9 x m x, 
E 0 0 0 l 00 
2(bC,, -aC, ) f, Š -2( a Cut FG) ^ 
Ins Ó x ]..4, 
cos( o, ) sin(g,)  x,cos( q,) — y,sin( の , ) 0 0 0 
-sin(g,) cos(e,) -y,cos(q,) - x,sin( の , ) 0 0 0 
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同样 ， 对 式 (5. 1). 采用 一 阶 差 商 的 方法 进行 离散 化 处 理 ， 得 到 离散 的 状 

态 空间 表达 式 : 
£y (k*1) = Aq (RE, (EE) + Bu CE) usu) (5.2) 

AP, 4』( を ) =I € TALL (CO , By (KR) = TBíGO) 。 

为 方便 读者 对 不 同 的 车 辆 动力 学 模型 进行 线性 化 处 理 ， 以 下 提供 本 书 在 处 
理 过 程 中 所 用 的 Matlab 代码 ， 程 序 为 chapter_ 5_ 1_ 1. m。 读 者 只 要 根据 建立 
的 车 辆 动力 学 模型 修改 程序 中 的 车 辆 固有 参数 和 微分 方程 ， 就 可 以 得 到 最 终 
结果 。 


clc 

clear all; 

名 名 以 下 为 程序 主体 

车 辆 参数 定义 

syms x dot y dot phi phi dot Y X;$ 车 辆 状态 量 

syms delta f  $ 控制 量 (前 轮 偏 角 ) 

S 车 辆 前 .后 轮滑 移 率 

SEZ0.2;5r 20.2; 

前 ,后 轮 距离 车 辆 质心 的 距离 ,车 辆 固有 参数 

a-21.232;b-1.468; 

S 前 、 后 轮 的 纵横 向 侧 偏 刚 度 ,车 辆 固有 参数 

Ccf =66900:Ccr=62700:C1f =66900 :C1r=62700: 

$ m 为 车 辆 质量 ,9 为 重力 加 速度 ,I 为 车 辆 绕 z 轴 转 动 惯量 ,车 辆 固有 
参 数 

11723 :2929.8;124175; 

S 车 辆 动力 学 模型 输入 

dy dot = -x dot* phi. dot+2* (Ccf * (delta f - (y. dot«a * 

phi. dot) /x dot)«Ccr * (b* phi, dot- y. dot) /x dot ) /m; 

dx dot-y. dot * phi dot«2 * (Clf * S£« Clr * Sr+ Ccf * delt 

a f* (delta f - (y dot-«phi dot * a) /x dot)) /m; 

dphi, dot-(2 * a * Ccf * (delta, f - (y dots a x phi. dot)/x.. 

dot)-2*b*Ccr * (b* phi. dot- y dot) /x dot) /I; 

Y dot-x dot * sin (phi)« y. dot * cos (phi); 

X dot- x dot * cos (phi)- y dot * sin (phi); 
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& 雅克 比 矩 阵 求解 
f = [dy. dot ;dx dot ;phi, dot ;dphi, dot ;Y dot;X dot]; 
S 动力 学 模型 
kesi = [y dot,x dot,phi,phi. dot,Y,X];$ 系统 状态 量 
v-delta f;$ 控制 量 
R =jacobian (f,kesi);% 矩阵 A(t) ,连续 形式 
R2 -jacobian(f,v);$ 和 矩阵 B (C) ,连续 形式 
# 将 转移 矩阵 由 连续 形式 转换 为 离散 形式 ,采用 一 阶 差 商 方法 , = エ +T* 
A(t),B-T*B(t) 
I-eye(6);* 单位 矩阵 
syms T;£ 采样 周期 
A-ZI«T*R; 
B-zT*R2; 
Al- vpa(A,5); 
Bl- vpa (B,5); 


5.1.2. 约束 条 件 建 立 


在 基于 动力 学 的 模型 预测 控制 器 设计 中 ， 除 了 需要 考虑 第 4 章 中 所 加 入 的 
控制 量 约束 以 及 控制 增 量 约束 外 ， 还 需要 添加 车 辆 动力 学 约束 ， 包 括 质心 侧 偏 
角 约 束 和 车 辆 附着 条 件 约束 ; 同时 ， 由 于 在 动力 学 模型 简化 过 程 中 ， 用 到 了 轮 
胎 的 线性 近似 表达 式 ， 因 此 还 需要 对 轮胎 的 侧 偏 角 进 行 限制 ， 以 保证 行驶 过 程 
中 轮胎 的 受 力 在 线性 区 域 中 ，. 

(1) 质心 侧 偏 角 约 束 

质心 侧 偏 角 对 车 辆 的 稳定 性 具有 比较 大 的 影响 ， 故 必须 将 质心 侧 偏 角 限 定 
在 侣 理 的 范围 内 。 博世 公司 (BOSCH) 所 进行 的 车 辆 稳定 性 研究 结果 T i 
ac 在 附着 良好 的 干燥 沥青 路 面 上 ， 车 辆 稳定 行驶 的 质心 侧 偏 攻 极限 可 以 达到 
+122， 而 在 附着 系数 较 低 的 冰雪 路 面 上 ， 极 限 值 近 似 为 上 2"。 因此， 本 书 将 
质心 侧 偏 角 的 约束 条 件 设 略为 : 

-12° < B < 12°( 良 好 路 面 ) (5.3) 
-2° < B < 2°( 冰 直路 面 ) 

(2) 车 辆 附着 条 件 约 束 

汽车 的 动力 性 能 不 仅仅 受到 驱动 力 的 制约 ， 还 受到 轮胎 与 地 面 附着 条 件 的 
旧制 ， 因 此 有 必要 添加 对 藻 辆 附着 条 件 的 约束 。 纵 向 加 速度 和 横向 加 速度 受到 
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地 面 附 着 力 的 限制 ， 存 在 如 下 关系 : 
+ X ug (5.4) 
式 中 ，a. 和 a, 分 别 为 纵向 加 速度 和 横向 加 速度 。 当 车 辆 纵向 匀速 行驶 时 ， 
式 (5.4) 可 以 进一步 简化 为 : 
| a, | S ug (5.5) 
当 行 驶 路 面 的 附着 条 件 较 好 时 ， 该 项 约束 条 件 较为 宽松 。 过 大 的 侧 向 加 速 
度 会 影响 到 人 的 乘坐 舒适 性 ， 但 约束 条 件 限定 过 罕 会 出 现 运算 过 程 中 求解 失败 
的 现象 。 为 了 综合 求解 质量 和 舒适 性 指标 ， 本 书 将 该 约束 设 定 为 软 约 束 条 件 ， 
即 求解 器 会 根据 每 个 控制 周期 的 求解 情况 动态 调整 该 约束 条 件 ， 如 式 (5.6) 
所 示 : 
a, wn 一 の SU, Saans tE (5.6) 
RP, a, min l we 为 加 速度 极限 约束 。 
(3) 轮胎 侧 偏 角 约束 
由 于 所 建立 的 车 辆 简化 动力 学 模型 没有 将 轮胎 侧 偏 角 作为 状态 量 ， 因 此 在 
每 一 步 的 控制 过 程 中 首先 需要 对 其 进行 求解 。 根 据 式 (2.35 ) ， 质 心 侧 偏 角 与 
状态 量 及 控制 量 存在 如 下 关系 : 
Q = try - ô; Q = EN 
在 任意 时 刻 :+， 系 统 各 状态 量 已 知 ， 轮 胎 侧 偏 角 的 求解 算式 为 : 


y, + ag, 


x x 


Qj = 一 Ci (5. 7) 


t 


y. - be, (5.8) 


Qp e 
X 


根据 轮胎 的 侧 偏 特性 可 知 ， 在 轮胎 侧 偏 角 不 超过 5° 时 ， 侧 偏 角 与 侧 偏 力 
为 线性 关系 。 根 据 第 2 章 对 非 线 性 动力 学 模型 在 小 角度 下 的 假设 ,将 前 轮 侧 偏 
角 的 约束 条 件 做 出 更 加 严格 的 限定 : 
=2 3 本 (5.9) 
需要 将 上 述 约束 条 件 纳 入 二 次 规划 的 求解 过 程 。 式 (5.7). 是 关于 状态 量 
和 控制 量 的 非 线性 函数 ， 再 一 次 利用 线性 化 方法 进行 求解 。 这 和 之 前 的 系统 线 
性 化 类 似 ， 唯 一 不 同 的 是 约束 条 件 并 不 需要 像 输出 量 那样 代入 到 目标 本 数 中 会 
与 最 优化 计算 。 以 前 轮 侧 偏 角 为 例 ， 线 性 化 采用 如 下 算式 : 
n ,Dz nn (5. 10) 
IË pa) OU Leo au 


の = 
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5.1.3 模型 预测 控制 器 设计 


在 上 一 章 ， 控制 右 的 目标 孔 数 选择 了 不 带 松弛 因子 的 形式 。 这 里 由 于 使 用 
的 是 车 辆 动力 学 模型 ， 故 复杂 程度 要 远 高 于 运动 学 模型 ， 同 时 也 加 入 了 更 多 的 
约束 条 件 ， 因 此 在 控制 器 实际 执行 过 程 中 ， 很 有 可 能 出 现在 规定 的 计算 时 间 内 
无 法 得 到 最 优 解 的 情况 。 因 此 ， 有 必要 在 目标 了 渔 数 中 加 入 松弛 因子 ， 表 达 式 
如 下 : 
JCE CO Wnt —1),AU,;,,(t)) = 


Nn 


VY.-l 
Y I LT * il L) = Tas seit *il t) lo + * | Au, (Ct *il t) || z + pe 
i=l 


“综合 上 面 的 目标 函数 和 约束 条 件 ， 基 于 动力 学 模型 的 主动 转向 跟踪 控制 器 
在 每 个 控制 周期 内 要 解决 如 下 的 优化 问题 ， 


Np 
min D | ma G 8 iE 0 quss +il E) e+ 
NM.-1 
Y | Aus +il 0 a * pe 
iz] 
s. t AU enin < AU, x AU ,,a max 
U irama < AAU iyn. t PM < C Loch 
Jn „min < Ji < Menai 


Jma =e e Ju zm Dismas +E 


g >0 (5. 11) 
式 中 ，y,, 为 硬 约束 输出 ， 即 不 能 放宽 约束 范围 的 输出 量 ; y. 为 软 约 束 输出 ， 
即 可 以 通过 松弛 因子 进行 动态 调整 约束 范围 的 输出 量 ; yw 和 ynw 为 硬 约束 
极限 值 ; yi 和 yy, 为 软 约束 极限 值 。 


在 每 个 控制 周期 内 完成 式 (5.11) 的 求解 后 ， 得 到 了 控制 时 域内 的 一 系 
列 控制 输入 增 量 和 松弛 因子 : 


AU si = [ AU iyn, Al iirst "ria SAM S, -I ,&] : (5. 12) 
将 该 控制 序列 中 第 一 个 元 素 作 为 实际 的 控制 输入 增 攻 作用 于 系统 ， 即 ; 
Wa (0) = (7 一 1) + Aus, (5. 13) 


进入 下 一 个 控制 周期 后 ， 重 复 上 述 过 程 ， 如 此 循环 实现 对 期 望 轨 迹 的 跟踪 
控制 
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5.2 联合 仿真 平台 搭建 


有 了 第 4 章 的 基础 ， 完 成 基于 动力 学 模型 的 主动 转向 控制 器 的 设计 将 会 更 
加 容易 。 整 个 联合 仿真 平台 依然 保持 着 原来 的 形式 (如 图 5.1 所 示 ) ， 主 要 由 
CarSim 提供 的 车 辆 动力 学 模块 和 5S 困 数 编写 的 控制 器 模块 组 成 。 而 在 CarSim 
中 主要 变化 在 于 车 辆 模型 的 建立 和 输入 、 输 出 接口 设 定 ， 在 控制 锅 模 块 中 需要 
根据 新 的 动力 学 模型 进行 调整 。 以 下 就 分 别 从 这 两 方面 进行 介绍 


Scope3 
we っ 
y do To Workspace 
fcn CarSim S-Function x. dot 
Vehicle Code: i i unde 
116 回 s 
Constant ea 


Unit Delay 


















Controller Dynamic CarSim 


XY Graph 





slip ratio 


S-Function 
delta_f 
ES Scope 
To Workspacel 


图 5.1 基于 动力 学 模型 的 CarSim/Simulink 联合 仿真 平台 


5.2.1 在 CarSim 中 建立 车 辆 模型 


本 章 中 需要 使 用 的 车 辆 动力 学 参数 如 表 S. 1 所 示 ， 主 要 为 车 辆 尺寸 信息 和 
轮胎 信息 。 因 此 ,我 们 在 设置 参数 时 首先 进入 车 辆 尺寸 参数 界面 。 其 设置 结果 
如 图 5. 2 所 示 。 车 辆 的 轮胎 选择 225/60 R18 ， 前 后 悬 架 都 选择 为 独立 悬 课 。 其 
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设置 结果 如 图 $.3 所 示 。 


表 5.1 车 辆 动力 学 参数 
车 辆 质量 /kg m=1723 








2X > he REA kg- m^) 1, 24175 
生身 尺 寸 /m L-4, W=1.988 


轴 距 xm a -1.232, b -1.468 
轮胎 侧 偏 刚度 /A(N > rad 7) Cer 266900, Cer 262 700 





i 
0 Lateral coordinate ol sprung mass center: ` Lahici enina 
122 | 
Mat center of sprung 
poc 


イオ sm 117 






The inertal properties are for the sprung mass in the design configuration. When this m checked more duaia meth modei wo ued | 
with no sdoona padno | | Frame Torsional Flexibity 
Sprung maet 1723 kg T Edi iadi ol gyrolon 

Riol inerba (ok} — 12431 kgm Fx 0849 m 

Pichineallw] — 43206 — kgm2 Ay) 1585 |m 

"am merba (1zz] 475 kgm? Rz 1557 m 
oduct ha? $ tga Inertia and radius o! graton are 
Produc ha} 0 kgm related by the equation | = M'R'R 


Produce! liyz} 0 kgm? 





图 5.2 车 辆 尺寸 参数 设置 


在 输入 输出 接口 方面 ， 由 于 只 对 车 辆 的 前 轮 进行 控制 ， 因 此 CarSim 的 输 
入 端口 选择 车 辆 4 个 轮胎 的 转角 。 其 中 2 个 前 轮 的 转角 由 控制 占 提 供 ，2 个 后 
轮转 角 固定 为 0。 其 设置 如 图 5.4 所 示 m 力学 系统 的 6 个 状态 
旺 和 2 个 前 轮 的 滑 移 率 ， 共 8 个 输出 量 。 其 设置 如 图 5.5 Bron. 


5.2.2 控制 程序 编写 


在 第 4 章 中 ， 我 们 已 经 采用 S 函数 编写 了 基于 车 辆 运动 学 模型 的 轨迹 跟 踊 
I dan. fEX—7 "t 中 ， 和 需要 继续 用 S p A adt V. Ti ii] du. Er 成 的 程序 为 chapter 
5 2 2.m. ERRON, MAR A Sepe PE C. al ibo [oS s El fA e 
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Suspension type: — independe — E. 





图 5.3 车 辆 悬 架 及 轮胎 设置 
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Active Import Variables 

Name Mode Initial Value 
2 IMP_STEER_R1 
3 IMP STEER L2 
IMP. STEER R2 














[85.4 CarSim 输入 端口 设置 


Active Variables for Export 





8. Xo 


7 Kappa L1 
8. Kappa R1 


图 5.5 CarSim fij Hi! ETE E 
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数 。 在 初始 化 函数 中 ， 主 要 改变 了 输入 与 输出 参数 的 个 数 ， 相 应 地 需要 对 系统 
状态 量 进行 初始 化 。 














function [sys,xO0,str,ts]-mdlInitializeSizes 


$ Call simsizes for a sizes structure,fill it in,and con- 
vert it 


© 


$ toa sizes array. 


sizes -simsizes; 


sizes.NumContStates  -0; 
sizes.NumDiscStates  -6; 
sizes.NumOutputs Es 
sizes.NumInputs Bs 


sizes.DirFeedthrough-1;$ Matrix D is non -empty. 

sizes.NumSampleTimes -1; 

sys-simsizes (sizes); 

x0 = [0.001;0.0001 ;0.0001;0.00001 ;0.00001 ;0.00001]; 

global U; 

U= [0];% 控制 量 初始 化 

% global x; 

$ x=zeros (md. ne +md. pye -md.me +md. Hu * md.me,1); 

% Initialize the discrete states. 

tr = Í]; % Set str to an empty matrix. 

ts = [0:050 ]} % sample time:[period,offset] 

% End of mdlInitializeSizes 

PETAI dH mdlOuputs (C), ， 需 要 完成 程序 基本 参数 初始 化 、 车 辆 
参数 输入 、 参 考 轨 迹 牛 成 、 二 次 规划 问题 求解 等 几 个 模块 。 

首先 介绍 第 一 部 分 ， 也 即 程序 参数 初始 化 ， 主 要 功能 是 设置 模型 预测 控制 
铝 的 基本 参数 ， 包 括 输 入 与 输出 个 数 、 预 测 和 控制 时 域 等 。 由 于 CarSim 中 米 
用 的 速度 単位 力 “km/h" ， 有 角度 单位 为 “?”， 而 我 们 在 $ 殴 数 中 采用 的 单位 分 
别 为 “mAs” 和 “rad”， 因 此 需要 加 入 对 输入 参数 的 转换 ， 而 这 一 点 往往 也 是 
我 们 最 容易 忽视 的 。 
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global a b; 

global U; 

tic 

Nx=6:* 状态 量 的 个 数 

Nu=1;% 控制 量 的 个 数 

Ny=2 ;% 输出 量 的 个 数 

Np-20;$ 预测 步 长 

Nc-5;$ 控制 步 长 

T-0.02;$ 仿真 步 长 

s 输入 接口 转换 ,x_qot 后 面 加 一 个 非常 小 的 数 , 是 防止 出 现 分 母 为 零 的 
情况 

y. dot-u(1y3.6; 

x dot- u(2)/3.6 + 0.0001; $ CarSim 输 出 的 是 kwh, 转换 
为 ms 

phi-u(3)*3.141592654/180;$ CarSim 输出 的 为 角度 ,角度 转换 为 
弧度 

phi. dot-u(4):*3.141592654/180; 

Y-u(5);$ 单位 为 m 

X=u(6);% 单位 为 mm 

Y dot-zu(7); 

X dot-u (8); 


预测 模型 是 模型 预测 控制 的 3 项 基本 原理 之 一 。 在 控制 器 中 需要 根据 我 们 
采用 的 动力 学 模型 设置 参数 ， 包 括 车 辆 轴 距 、 轮 胎 刚度 以 及 车 辆 质量 等 ， 有 具体 
代码 如 下 : 





を る 车 辆 参数 输入 

S 前 后 车 轮 的 滑 移 率 

S£-0.2;Sf20.2; 

S 前 后 车 轮 距离 车 辆 质心 的 距离 ,车 辆 固有 参数 
1fz1.232;1r-1.468; 
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: 分别 为 前 后 车 轮 的 纵横 向 侧 偏 刚度 ,车 辆 固有 参数 

cf=66900 :Ccr=62700 :C1F=66900 :C1r=62700 ; 

为 车 辆 质量 ,g 为 重力 加 速度 , 工 为 车 辆 绕 z 轴 的 转动 惯量 ,车 辆 固有 
参数 


m-1723;gz29.8;1-24175; 


完成 车 辆 动力 学 参数 设置 后 ， 需 要 给 出 参考 轨迹 。 在 仿真 过 程 中 ， 既 可 以 
将 参考 轨迹 生成 的 部 分 放 在 控制 器 中 ， 也 可 以 单独 提供 一 个 模块 并 建立 与 控制 
加 的 接口 .本文 将 参考 轨迹 生成 的 部 分 放 在 控制 器 中 ， 而 有 兴趣 的 读者 也 可 以 
自己 尝试 外 加 模块 ， 以 实现 同样 功能 。 参 考 轨 迹 的 售 义 将 在 仿真 结果 部 分 介 
绍 ， 以 下 仅 给 出 实现 代码 : 





s 参考 轨迹 生成 

shape-2.4; $ 参数 名 称 , 用 于 参考 轨迹 生成 

àx12z25;d40x2221.95; 

dy124.05;dy225.7; 

Xs1=27.19:Xs2=56.46:$ 参 数 名 称 

X predict- zeros (Np,1) ;% 用 于 保存 预测 时 域内 的 纵向 位 置信 息 .这 是 
计算 期 望 轨迹 的 基础 

phi ref-zeros (Np,1) : 用 于 保存 预测 时 域内 的 期 望 轨迹 

y_ref=zeros (Np,1) : 用 于 保存 预测 时 域内 的 期 望 轨迹 

dphi_ref= zeros Mesue: 


上 述 步 骤 都 可 以 被 看 成 建立 模型 预测 控制 器 的 准备 工作 ,而 线性 时 变 模型 
预测 控制 的 主要 工作 是 根据 线性 时 变 模 型 预测 系统 输出 ,结合 系统 约束 ,将 车 辆 
轨迹 跟踪 问题 转换 为 标准 二 次 规划 问题 进行 求解 。 线 性 时 变 模 型 预测 控制 的 算 
式 推导 部 分 可 以 参考 本 书 3.3 节 。 读 者 在 完成 了 3.3.3 中 的 实例 程序 后 ,再 结 
侣 本 章 中 对 动力 学 模型 的 线性 化 求解 ,就 可 以 比较 顺利 地 完成 以 下 程序 : 





* 天 阵 转换 , 即 状态 量 与 控制 贡 合 在 一 起 
kesi= zeros (Nx -Nu,1); 

kesi (1)2 y. dot;$ u(1)== X (1) 

kesi (2)= x dot;$ u(2)-- X (2) 
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kesi 3)- phi;$ u (3-- X (3) 
kesi (4)= phi dot; 

kesi (5)- Y; 

kesi (6)= X; 

kesi (7)=U (1); 

delta f-U(1); 


Q cell-cell(Np,Np; 
for iz1:1:NDp: 
for jz1:1:Np; 
if i=j 
Q cell(i,j)-[2000 0;0 10000;]; 
else 
Q cellíi,j)-zeros (Ny,Ny); 
end 
end 
end 
R=5*10^5 * eye (Nu * Nc); 
Row -1000;2 松弛 因子 权重 


a=[1 - (259200 «x T) /(1723 * x. dot), = T * (phi. dot + (2 * 
((460218 * phi. dot )/5- 62700 * y dot)Y/(1723 * x dot ^2) - 
(133800 * ((154 * phi. dot /125« y. dot) (1723 * x dot ^2)), 
0, -T* (x dot-96228/(8615 * x dot)),0,0 


T * (phi dot - (133800 * delta £)/(01723 * x dot)),(133800 * T * 
delta f* ((154 * phi dot)/125«y dot))/(1723 * x dot ^2) 


*1,0,T* (y dot - (824208 x delta f)/(8615 * x dot)),0,0 


0,0,1,T,0,0 
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(33063689036759 * T)Yy(7172595384320 * x dot), T æ 
(((2321344006605451863 * phi _ dot )/ 8589934592000 - 
(6325188028897689 * y, dot /34359738368) /(4175 * x, dot^2) + 
(5663914248162509 * ((154 * phi, dot)/125 + y . dot) )/ü 
43451907686400 * x dot^2)),0,1, - (813165919007900927 x 
T)/(7172595384320000 * x dot),0,0 


T*cos(phi),T*sin(phi), T * (x dot * cos (phi)- y dot * sin 
(phi)),0,1,0 


-T* sin(phi),T * cos (phi), -T * (y. dot * cos (phi)* x dot * 
sin(phi)),0,0,1]; 


b=[ 133800 *T/1723 
T+ ((267600 * delta, f /1723 - (133800 * ((154 * phi, dot)/ 





125 +y_dot ) )/(1723 * x dot)) 
0 
5663914248162509 * T/143451907686400 
0 
0]; 


A cell-cell (2,2); 
B_ce11 =ce1 1 (2,1); 
A cell(l,l)-a; 

A_ce1 1 {1 ,2} =b: 

A cellí2,1)-2zeros (Nu , NX) 

A cellí(2,2)-eye (Nu); 

B celti[l,lkzb: 

B cell(2,1)-eye(Nu); 
AÀ-cell2mat (A cell); 
B-cell2mat (B cell); 
Ca[0010080;000010071; 


5rjs.cn 000000 


无 人 驾驶 车 辆 模型 预测 控制 


d k =zeros (Nx,1); 

state_k1 = zeros (Nx,1);$ 预测 的 下 一 时 刻 状态 量 , 用 于 计算 偏差 

根据 离散 非 线 性 模型 预测 下 一 时 刻 状态 量 

state _k1 (1,1)=y_dot +T* (-x dot * phi_dot+2 * (Ccf * 
(delta f - (y dot +l1fx*phi dot)x dot)+Ccr * (lr * phi 
_dot - y. dot x dot ym); 

state k1 (2,1)- x dot-T * (y. dot * phi, dot «2 * (Clf * S£«Clr 
* Sr --Ccf * delta f * (delta f - (y dot «phi dot * 1 
f yx dot)Yym); 

state k1 G,1)2phi +T* phi dot; 

state kl (4,1)»phi dot «T * ((2 * 1f * Cc£ * (delta. f - (y. dot 
*lf*phi dot)x dot)-2 * lr *Ccr * (lr*phi dot-y d 
ot yx Aot Ir; 

state k1 (5,1) 2-Y +T = (x dot * sin (phi)«y dot * cos (phi)); 

state k1 (6,1)2 X« T * (x dot * cos (phi)-y dot * sin (phi)); 

d k-2state kl-a*skesi(1:6,1)-b*kesi(7,1); 

d piao k = zeros (Nx «Nu,1);$ d_k 的 增 广 形 式 

d piao k(1:6,1)=d_k; 

d piao k(7,1)-0; 


PSI cell-cell(Np,1); 
THETA_ce] 1] = cell (Np ,No) : 
GAMMA cell-cell (Np, Np : 
PHI. cell-cell(Np,1); 
for pz1:1:Np; 
PHI cellíp,1)-d piao k; 
for qz1:1:Np; 
if q<=p; 
GAMMA cellíp,q)-C*A^(p-q); 
else 
GAMMA cellíp,qd) -zeros (Ny,Nx + Nu ) : 
end 
end 


end 
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for j =1 :1 :Np 
PSI cell(j,1)sC*A^j; 
for kz1:1:Nc 
lif Kec 
THETA cell(j,k)-2C*A^(j -k)*B; 
else 
THETA cellíj,k)-zeros (Ny,Nu); 
end 
end 

end 
PSI-cell2mat (PSI cell);$ size (PSI) = [Ny * Np Nx + Nu] 
THETA = cell2mat (THETA, cell); $ size (THETA) = [Ny * Np Nu * 
Nc] 
GAMMA = cell2mat (GAMMA, cell);$ 大 写 的 GAMMA 
PHI = cell2mat (PHI cell); 
O =ce112mat (Q cell); 
H cell-cell (2,2); 
H cell(1,1)- THETA * Q * THETA +R; 
H cellí1,2)-2zeros (Nu * Nc,1 ) : 
H cell(2,1)-zeros(1,Nu-* Nc); 
H cell(2,2)-2Row; 
H 2cell2mat (H cell); 


error 1 -zeros (Ny *Np,1); 
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Yita ref cell-cell(Np,l); 





$ 获得 局 部 参考 轨迹 ,注意 防止 越界 问题 





T_all=20;% 总 的 仿真 时 间 ,主要 功能 是 防止 计算 期 望 轨迹 越界 
for p -1:1:Np 
if t«p*T»T all 
X DOT-x dot * cos (phi)- y dot * sin (phi);$ 惯性 坐 
标 系 下 纵向 速度 
X predict (Np,1) =X «X DOT * Np * T; 
zl = shape /dx1 *(X predict (Np,1)- Xs1)- shape/2 ; 
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z2-shape/dx2 *(X predict (Np,1)- Xs2)- shape/2; 
Y ref (p,l)-dy1/2 *(1 + tanh (z1))- dy2/2 * (1-4 tanh 
(z2 ) ) : 
phi ref (p,1)= atan (dy1 * (1 /Cosh (z1) ) ^2 * (1.2 /dx1)- 
dy2 * (1Cosh (z2 ) ) 2 * (1.2 /dx2)); 
Yita ref cell(íp,1)-[phi ref (p,1);Y ref (p,1 ) ] : 
else 
X DOT -x dot * cos (phi)- y. dot * sin (phi);% 惯性 坐标 系 
下 纵向 速度 
X. predict (p,1)=X+X_DOT*p*T:s 首先 计算 出 未 来 x 的 位 
H.,X(O-SX-X dot*t 
zl = shape/dxl * (X predict (p,1) -Xs1) -shape/2; 
z2 -shape/dx2 * (X predict (p,1) -Xs2) -shape/2; 
Y ref (p,1)-dy1/2 * (1 + tanh (z1)) - dy2/2 * (1 + tanh 
(z2 ) ) : 
phi _ref (p,1) = atan (dQy1 * (1 Cosh (z1)) ^2 * (1.2/dx1 ) 
-dy2 * (1/cosh (z2))^2 * (1.2/dx2 ) ) : 
Yita, ref cellíp,1)- [phi. ref (p,1) ;Y. ref (p,1)]; 
end 
end 
Yita ref-cell2mat (Yita ref cell); 
error 1 -Yita ref -PSI * kesi -GAMMA * PHI ; $ 求 偏差 
f cell-cell(1,2); 
f cellí(1,1)2.2 * error 1 * Q * THETA; 
f cellí(1,2)-0; 
f = -cell2meat (f cell); 


在 上 面 的 程序 中 ,我 们 已 经 获得 了 二 次 规划 问题 中 至 关 重 要 的 两 个 矩阵 五 
和 ff， 而 接 下 来 就 是 要 根据 需要 的 约束 设 定好 约束 条 件 ， 包 括 控制 量 约束 、 控 
制 增 量 约束 和 输出 量 约 束 。 





名 名 以 下 为 约束 生成 区 域 
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を 控制 量 约束 


A t-zeros (Nc,NC); 
for p =1 :1 :Nc 
for q-1:1:Nc 
ifq«-p 
A t (p,q)-1; 
else 
A t (p,q) 20; 
end 
end 
end 
A I-kron(A t,eye (Nu)) を 求 克 罗 内 克 积 
Ut = kron (ones (Nc ,1) ,U (1) ) : 
umin =-0.1744;% 维 数 与 控制 变量 的 个 数 相同 
umax -0.1744; 
delta umin --0.0148; 
delta umax -0.0148; 
Umin = kron (ones (Nc,1),umin); 


Umax - kron (ones (Nc,1),umax); 


ycmax [0.2175]; 
ycminz[-0.3;-23]; 
Ycmax = kron (ones (Np,1),ycmax) ; 


Ycmin - kron (ones (Np,1),ycmin); 


- デー ニー ニー ニニ テニ ニニ ニー ニニ ニニ ーー ニニ ニニ ニニ ニー ニニ ニー ニニ ニニ ーーー ニー ニー デー ニー デー ニー デー ニー ニー ニーー ニ ーー ニー ニー デー ニー デー デーーーーー 
* 结合 控制 量 约束 和 输出 量 约束 
名 ニニ ニニ ニー ニニ ニー ニー ニニ ニニ ニニ ニー ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ 


A cons cell- (A. I zeros (Nu * Nc,1); -A_I zeros (Nu * Nc, 1); 
THETA zeros (Ny * Np,1) ; - THETA zeros (Ny * Np,1)); 
b cons cell = {Umax - Ut; - Umin + Ut ; Ycmax - PSI * kesi -GA 


MMA * PHI; - Ycmin + PSI * kesi-« GAMMA * PHI}; 
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A cons -cell2mat (A cons cell);$ (求解 方程 ) 状 态 量 不 等 式 约束 增 
dB E 

b. cons = cell2mat (b cons cell);$ (求解 方程 ) 状 态 量 不 等 式 约束 的 
取 值 

gs 状态 量 约束 

M -10; 

delta Umin -kron (ones (Nc,1),delta umin); 

delta Umax- kron (ones (Nc,1),delta umax); 


lb = [delta Umin;0];$* (求解 方程 ) 状 态 量 下 界 , 包 含 控制 时 域内 控制 


兽 量 和 松弛 因子 
ub = [delta Umax;M];$ (求解 方程 ) 状 态 量 上 界 , 包 含 控 制 时 域内 控制 
增 量 和 松弛 因子 


最 后 ， 结 合 二 次 规划 的 标准 矩阵 和 约束 条 件 进 行 最 优 求解 。 需 要 完成 的 工 
作 包 括 选择 优化 方法 (一般 在 有 效 集 与 内 点 法 之 间 选 择 )、 设 定 初始 点 等 。 求 
解 结束 后 ， 根 据 模型 预测 控制 的 基本 原理 ,将 控制 增 量 序列 的 第 一 个 元 素 与 上 
一 个 时 刻 的 控制 量 相 加 ， 输 出 给 CarSim 动力 学 模块 


options = optimset ('/Algorithm/ active - set'/);$ 选取 有 效 

x start- zeros (Nc«1,1); 

[X,fval,exitflag]- quadprog (H,f,A, cons,b. cons, [], (], 1b, 
ub,x start,options); 

s る 计算 输出 

u piao-X(1;$ 得 到 控制 增 量 

U1)=kesi (7,1)+u_piao;% 当前 时 刻 的 控制 量 为 上 一 时 刻 控制 + 控制 

增 量 
sys -U;$ 系统 输出 
s 程序 结束 


至 此 ,我 们 就 完成 了 基于 动力 学 模型 的 主动 转向 控制 器 的 设计 。 读 者 可 以 
分 段 将 程序 读 懂 ， 然 后 将 各 段 程序 合 在 一 起 ， 在 联合 仿真 平台 上 测试 程序 的 功 
能 。 以 下 将 介绍 本 书 针对 无 人 驾驶 车 辆 的 行驶 工 况 所 设计 的 仿真 环境 及 验证 
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5.3 仿真 验证 


5.3.1 参考 轨迹 选择 


基于 动力 学 模型 的 模型 预测 控制 器 主要 用 于 较 高 速度 下 的 轨迹 跟踪 。 在 这 
种 情况 下 对 于 控制 器 的 评价 不 仅仅 是 跟踪 精度 ， 还 需要 重点 考虑 跟踪 过 程 中 的 
稳定 性 .在 普通 车 辆 行驶 稳定 性 的 测试 中 ， 双 移 线 工 况 是 使 用 频率 较 高 的 一 种 
测试 路 段 “”- 国内 外 也 有 较 多 的 学 者 以 双 移 线 轨 迹 进 行 无 人 驾驶 车 辆 轨迹 
跟踪 能 力 的 测试 ”-”。 因 此 ， 本 文 参考 文献 [77] 中 的 双 移 线 轨迹 ， 对 所 设 
计 的 模型 预测 控制 器 进行 仿真 测试 

双 移 线 轨迹 如 图 5.6 所 示 ， 由 参考 横向 位 置 Y, 和 参考 横 摆 角 wu 构成 。 
Y. Fl yp, 都 表示 为 关于 横向 位 置 X 的 非 线 性 函数 ， 具 体 算式 如 下 : 


d da 
FAAI S x 1 + tanh(z,)) - e 1 + tan( =。) ) (5. 14) 


e. = aretan( d, ( = ) itat] Ga (5.15) 


cosh( z, ) 
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5.3.2 不 同 仿真 工 况 下 的 仿真 结果 


所 设计 的 主动 转向 跟踪 控制 器 以 车 辆 前 轮 偏 角 作 为 控制 器 的 输入 ， 控 制 目 
标 是 通过 不 断 减 少 与 参考 轨迹 的 偏差 ， 跟 踪 参 考 轨迹 。 为 了 更 加 全 面 地 分 析 所 
提出 控制 方法 的 可 行 性 ， 本 书 设计 3 种 不 同 的 仿真 工 况 ， 尽 可 能 体现 控制 器 在 
实际 运用 时 可 能 过 到 的 各 种 情况 。 

(1) 仿真 工 况 1: 控制 系统 对 路 面 附 着 条 件 的 鲁 棒 性 

无 人 驾驶 车 辆 在 不 同 附着 条 件 的 道路 上 ( 如 干燥 路 面 、 湿 滑 路 面 ) 行驶 
时 ， 车 辆 自身 的 动力 学 参数 ， 如 轮胎 侧 偏 刚度 等 将 会 发 生变 化 ， 同 时 也 会 出 现 
地 面 提供 的 侧 向 力 不 足 的 情况 。 这 给 控制 絮 的 性 能 带 来 了 一 定 的 挑战 。 仿 真 工 
况 1 就 是 在 不 同 附 着 条 件 的 道路 上 对 车 辆 轨迹 跟踪 能 力 进行 测试 。 无 人 驾驶 车 
辆 正常 行驶 路 面 的 附着 系数 为 人 =0.7 ~1， 湿 滑 路 面 的 附着 系数 为 =0.4 7c 
右 。 因 此 ， 分 别 选取 j=0.4，0. 8 两 种 情况 进行 测试 。 

车 辆 纵向 速度 设 定 为 30 m/s， 轨 迹 跟 踪 控 制 器 的 基本 参数 设 定 为 : 了 = 
0.05s, N, =25, N, 215, -10?«8,«10?, -0.85?« Aó,«0.85^, BUE RI 
设置 为 : 


200 0 0 
o -| 0 100 o | R=5x10, p= 1000. 
0 0 100 

仿真 结果 如 图 S.7 所 示 。 其 中 ,图 5.7 (a) 为 系统 输出 量 [Y、e] 随时 间 
的 变化 关系 ,图 3.7 (b) 为 系统 控制 量 o, 和 控制 增 量 Ao, 随时 间 的 变化 关系 ， 
图 5.7 (e) 为 所 设 定 的 动力 学 约束 条 件 在 仿真 过 程 中 的 变化 。 从 图 5.7 (a) 中 
曲线 可 以 看 出 ， 控 制 器 在 不 同 附着 条 件 的 路 面 上 都 能 很 好 地 跟踪 期 望 轨 迹 ， 但 附 
着 条 件 良 好 时 车 辆 的 跟踪 误差 能 够 进一步 缩小 。 若 附着 条 件 较 差 ， 地 面 不 能 提供 
足够 的 侧 向 力 ， 车 辆 转向 时 横 摆 角 会 出 现 较 大 偏差 (图 5.7 (a) 中 70 m 4b), 
轨迹 跟踪 控制 器 能 够 及 时 修正 偏差 ， 最 终 将 偏差 收敛 为 0。 从 图 $.7 (b) 中 可 
以 看 出 ， 跟 踪 过 程 中 车 辆 的 控制 量 以 及 控制 增 量 都 处 于 约束 范围 内 ， 保 证 了 给 定 
车 辆 的 控制 量 能 被 执行 机 构 顺 利 执行 。 图 $.7 (e) 中 ， 除 了 设 定 为 软 约束 的 纵 
向 加 速度 外 ， 各 项 约束 都 被 限定 在 了 给 定 的 区 间 范 围 内 。 软 约束 的 加 入 保证 系统 
能 够 在 给 定时 间 内 得 到 可 行 解 。 综 合 上 述 分 析 ， 控 制 系统 能 够 在 不 同 附着 条 件 
下 ， 较 好 地 跟踪 期 望 轨迹 ， 并 具备 良好 的 稳定 性 。 

(2) 仿真 工 况 2: 控制 算法 对 速度 的 鲁 棒 性 

很 多 控制 算法 往往 需要 针对 不 同 的 行 怠速 度 确定 不 同 的 控制 参数 ， 而 模型 
预测 控制 器 能 够 根据 建立 的 车 辆 模型 预测 系统 未 来 的 输出 ， 对 车 速 的 变化 具有 
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图 5.7 仿真 工 况 1 的 仿真 结果 
(a) 输出 量 随时 间 变 化 历程 ; (b) 控制 量 随 时 间 变 化 历程 ; (c) 约束 量 随 时 间 变化 历程 


强 的 鲁 棒 性 。 仿 直 工 况 2 就 是 在 不 同 的 车 速 下 ， 以 相同 参数 的 控制 器 实现 对 
驾驶 车 辆 的 主动 转向 控制 ， 分 析 控 制 器 对 不 同 车 速 的 鲁 棒 性 。 
E 动 转向 仿 直 实验 分 别 在 10 m/s, 20 m/s 和 30 m/s 的 速度 下 进行 ， 道路 


附着 条 件 良 好 , 20.8. 控制 器 所 采用 的 参数 为 : T=0.05 s，N =25，AN. = 


10, 


-]0?«68,-10^, -0. 85° < A8, <0. 85°, IUE KEI TL: 


200 0 0 
Q = | 0 100 0 | =l.ix10'、p=1000 
0 0 100 
仿真 工 沈 2 up; res e n 5.8 所 示 。 其 中, [85.8 Ca) 为 3 种 不 同和 车 


速 下 车 辆 的 轨迹 跟踪 对 比 ， 图 $.8 (b) 为 控制 量 和 控制 培 基 随时 间 变 化 山 线 ， 


5rjs.cn 000000 


131 


无 人 驾驶 车 辆 模型 预测 控制 


图 5.8 (c) 为 动力 学 约束 随时 间 变 化 曲线 ， 实 线 、 点 线 和 点 画 线 分 别 对 应 车 
速 为 10 m/s, 20 m/s 和 30 m/s 的 仿真 结果 。 从 图 5.8 (a) 可 以 看 出 ， 在 相同 
控制 参数 下 ， 不 同 速度 行驶 的 车 辆 都 具备 良好 的 轨迹 跟踪 性 能 ， 体 现 出 对 速度 
很 强 的 鲁 棒 性 。 图 5.8 (b) 中 , 不 同 速度 下 控制 量 的 增 量 差异 较 大 ， 速 度 越 
高 ， 控 制 量 增 量 就 越 大 ， 但 始终 保持 在 约束 范围 内 。 图 5$.8 (e) 中 各 动力 学 
约束 保持 在 给 定 的 区 间 范 围 内 ， 质 心 侧 偏 角 则 远 低 于 极限 范围 ， 表 明和 车 辆 行驶 
过 程 非常 平稳 。 因 此 ， 本 书 所 设计 的 轨迹 跟踪 控制 带 能 够 在 不 同 车 速 下 完成 对 
期 望 轨 迹 的 跟踪 ， 车 速 增加 并 不 会 导致 车 辆 稳定 性 能 的 下 降 。 
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图 5.8 仿真 工 况 2 的 仿真 结果 
(a) 输出 量 随 时 间 变 化 历程 ; (b) 控制 量 随时 间 变 化 历程 ，(e) 约束 量 随时 间 变 化 历程 


(3) 仿真 工 况 3: 不 同 设计 参数 对 控制 器 的 影响 
不 同 参数 对 控制 器 性 能 的 影响 有 所 差异 。 对 于 模型 控制 算法 来 说 ， 最 重要 
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的 设计 参数 就 是 预测 时 域 与 控制 时 域 。 为 了 考察 不 同时 域 对 控制 器 跟踪 性 能 和 
计算 量 的 影响 ， 分 别 设 计 了 控制 器 A 和 控制 器 B。 控 制 器 A 所 采用 的 参数 如 
F: 7=0.05 s，N 225, N, 210, - 10? «à, «10*, —0.85? « Aó, «0. 85*, 
u =0. 8, ECEGBPEUE BEA: 


200 0 0 
2- o 100 o | R-5x10, p = 1000 
0 0 100 
相 比 控制 器 A、 控 制 器 B 减少 了 预测 时 域 和 控制 时 域 的 步 长 ， 设 定 N, = 
I5. V =1 ,其 余 参 数 与 控制 器 A 保持 一 致 。 分 别 在 不 同 的 控制 器 作用 下 ， 验 


证 无 人 驾驶 车 辆 轨迹 跟踪 及 保持 稳定 行驶 的 能 力 。 

仿真 工 况 3 的 仿真 结果 如 图 5.9 所 示 。 其 中 , 图 5.9 (a) 为 两 个 控制 跟 
蒜 期 望 轨迹 的 对 比 ， 图 5.9 (b) 为 两 个 控制 器 在 每 个 周期 内 的 计算 时 间 ， 图 
中 实 线 和 点 线 分 别 对 应 控制 器 A、B 的 仿真 结果 。 从 图 5.9 (a) 可 以 看 出 ， 控 
Mar B 能 够 跟踪 期 望 轨 迹 ， 并 且 最 终 能 够 将 偏差 收敛 为 0。 与 控制 器 A 的 跟踪 
效果 相 比 ， prj o ln er 尤其 是 对 期 望 横 损 角 的 跟踪 ;但 控制 器 
B 并 不 是 一 无 是 处 ， 图 5.9 (b) 中 的 曲线 可 以 非常 直观 地 表示 出 ， 控 制 器 B 
Veiis od A, 平均 只 有 不 到 10ms。 因 此 ， 采 用 较 小 的 预测 
时 域 和 控制 时 域 对 于 提升 控制 系统 的 实时 性 效果 明显 。 在 硬件 条 件 较 低 而 又 期 
望 保证 系统 的 实时 性 情况 下 ， 可 以 采用 这 样 的 设计 方法 。 


一 一 控制 器 A 


Y/m 
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图 5.9 (frt ro53 的 仿真 结果 
(a) Mí bti apte fe Xs (b) 不 同 控制 器 的 计算 时 间 
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本 章 介绍 地 面 无 人 驾驶 车 辆 结合 路 径 规划 层 的 轨迹 跟踪 控制 器 。 车 辆 类 型 
选择 前 轮转 向 的 乘 用 汽车 。 首 先 介 绍 了 基于 车 辆 点 质量 模型 的 MPC 轨迹 规划 
控制 器 ， 规 划 出 满足 车 辆 动力 学 约束 并 实现 避 障 的 可 行 轨迹 ; 然后 介绍 了 在 车 
辆 动力 学 模型 的 基础 上 的 MPC 路 径 跟踪 控制 器 ， 并 给 出 了 SEXES 最 后 
详细 介绍 了 利用 Simulink/CarSim 进行 联合 仿真 的 实例 。 

无 人 驾驶 车 辆 在 实际 环境 中 进行 自主 驾 台 时 ， 由 于 外 部 环境 是 动态 的 、 变 
化 的 ， 所 以 给 定期 望 轨迹 下 的 跟踪 控制 并 不 能 保证 无 人 驾驶 车 辆 准确 地 处 理 任 
何 轨 迹 跟踪 问题 。 原 因 如 下 : 

D 由 于 环境 未 知 ， 预 先 给 定 的 参考 路 径 不 一 定 能 满足 安全 行驶 的 要 求 

(2) 当 所 给 定 的 期 望 轨迹 存在 障碍 物 时 ， 无 人 驾驶 车 辆 要 根据 障碍 物 信息 
进行 重新 规划 ， 绕 开 障 碍 物 后 再 继续 跟踪 期 望 轨迹 。 

③ 为 了 减少 计算 量 ， 在 路 径 跟踪 过 程 中 使 用 的 车 辆 模型 是 经 过 简化 的 ， 
给 定 的 参考 轨迹 不 一 定 能 满足 车 辆 行驶 的 动力 学 和 运动 学 的 约束 条 件 。 

因此 本 章 从 轨迹 重 规划 的 角度 出 发 ， 在 轨迹 跟踪 层 之 上 建立 轨迹 重 规划 
层 ， 也 即 局 部 规划 层 。 该 规划 层 能 够 根据 传感器 获得 的 障碍 物 信息 和 参考 路 径 
信息 重新 规划 出 绕 开 障碍 物 的 局 部 期 望 轨迹 信息 ， 再 将 局 部 期 望 轨迹 信息 输入 
跟 踊 控制 屋 ， 在 实现 避让 障碍 物 的 同时 ， 实 现 对 全 局 参考 路 径 的 跟踪 


6.1 结合 规划 层 的 轨迹 跟 踊 控制 系统 


在 原 有 的 轨迹 跟踪 控制 基础 上 加 入 规划 层 后 ， 形 成 新 的 控制 系统 ， 结 构 
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如 图 6. 1 所 示 。 该 控制 系统 主要 由 带 避 障 功 能 的 轨迹 重 规划 模块 和 跟踪 控制 
模块 构成 。 跟 踪 控 制 模块 接收 来 自 规划 层 的 局 部 参考 轨迹 ， 输 出 前 轮 偏 角 控 
制 量 ; 而 轨迹 重 规划 模块 接收 来 自传 感 器 的 障碍 物 信息 以 及 来 自 全 局 规划 的 
参考 轨迹 信息 ， 通 过 模型 预测 控制 算法 规划 出 局 部 参考 轨迹 ， 再 发 送 给 跟踪 
控制 模块 . 





无 人 驾驶 车 辆 


图 6.1 结合 规划 层 的 轨迹 跟踪 控制 系统 


基于 MPC 的 轨迹 规划 层 需 要 满足 以 下 3 个 条 件 : 

规划 得 到 的 期 望 轨迹 与 给 定 的 参考 路 径 之 间 的 偏差 要 尽 可 能 小 。 

D 规划 得 到 的 期 望 轨迹 要 满足 车 辆 的 运动 学 和 动力 学 的 约束 条 件 。 

D 规划 得 到 的 期 望 轨迹 要 能 够 避 开 障碍 物 ， 满 足 安全 避 障 要 求 。 

基于 MPC 的 轨迹 跟踪 控制 层 需要 满足 以 下 3 个 条 件 : 

D 实际 的 跟踪 轨迹 与 期 望 轨迹 之 间 的 偏差 要 尽 可 能 小 。 

D 优化 计算 得 到 的 车 辆 控制 量 要 满足 执行 机 构 的 限制 。 

3) 车 辆 行驶 要 满足 安全 性 要 求 ， 这 里 指 车 辆 行驶 时 不 发 生 打 滑 或 者 
侧 翻 
在 所 构建 的 双 层 控制 体系 中 都 采用 模型 预测 控制 算法 ， 因 此 需要 确定 两 方 
面 的 问题 : 

(1) 规划 层 与 控制 层 的 模型 选择 

车 辆 模型 的 选择 需要 综合 考虑 性 能 表现 以 及 计算 量 等 因素 。 由 于 规划 算 
法 本 身 算法 计算 量 大 ， 而 引入 模型 预测 控制 的 主要 目的 是 使 规划 结果 要 满足 
车 辆 动力 学 和 运动 学 模型 约束 ， 因 此 为 减少 计算 量 ， 不 宜 采 用 太 复 杂 的 模 
n». App xk [70] 的 对 比 实验 可 知 ， 在 规划 层 采 用 较 低 精 度 的 模型 而 在 
控制 层 采 用 较 高 精度 的 模型 ， 能 够 较 好 地 兼顾 控制 性 能 和 计算 速度 ， 因 此 ， 
在 规划 层 中 采用 忽略 车 身 尺 寸 信息 的 点 质量 模型 。 
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(2) 控制 周期 选择 

一 般 来 说 ， 规 划 层 的 规划 周期 可 以 比 控制 层 的 控制 周期 长 。 这 主要 是 因 
为 规划 层 一 次 可 以 规划 出 包含 若干 个 控制 周期 的 轨迹 ， 而 采用 较 短 的 规划 周 
期 对 提升 规划 性 能 并 没有 帮助 。 在 此 ， 将 规划 层 的 规划 周期 取 为 控制 层 控制 
周期 的 两 倍 ， 经 过 后 续 仿 真实 验 验证 ， 该 设 定 可 以 保证 控制 系统 的 实时 性 与 
可 靠 性 。 


6.2 基于 MPC 的 轨迹 规划 器 


轨迹 重 规划 算法 是 轨迹 规划 层 中 的 核心 内 容 ， 主 要 工作 是 设计 合理 的 评价 
函数 ， 在 满足 各 种 约束 条 件 下 ， 实 现 避 障 功能 并 且 尽 量 减 小 车 辆 与 全 局 参考 路 
径 的 偏差 .最 后 通过 合理 的 方式 输出 给 跟踪 控制 层 ， 

在 规划 层 中 采用 点 质量 模型 ， 如 图 6. 2 所 示 。 根 据 第 2 章 可 以 得 到 车 辆 的 


点 质量 模型 ， 如 式 (6.1) 和 式 (6.2) 所 示 。 


图 6.2 点 质量 模型 示意 图 


Y = a, 

= 0 

jut (6. 1) 
X 


Y = tsing + ycosp 
X = cos の 一 ysing 
考虑 车 辆 的 动力 学 约束 ， 加 入 如 下 约束 条 件 : 
lal<ug (6.2) 
X (6.1) 和 式 (6.2) 可 以 简写 成 如 下 形式 : 
EU) = を (の (の ) ) 


(6. 3) 
| u(0 1 « ug 


5rjs.cn 000000 


o 第 6 章 加 入 规划 层 的 轨迹 跟踪 控制 





式 中 、 ま = [y, €, d, Y, X] ， 共 有 5 个 离散 的 状态 变量 ， 分 别 为 车 辆 在 y 
fx 方向 的 车 速 、 车 辆 航向 角 、 车 体位 置 的 纵 坐 标 和 横 坐 标 。u 代表 控制 量 ， 
此 处 为 车 辆 的 前 轮 偏 角 5 

6.2.1 参考 点 的 选择 

在 轨迹 重 规划 的 时 候 震 计算 参考 轨迹 与 预测 轨迹 之 间 的 偏差 了 -nw， 而 
起 始 参考 轨迹 点 如 何 选取 则 是 其 中 的 一 个 关键 问题 。Eklund 等 利用 几何 
法 寻找 全 局 参考 轨迹 上 离 车 辆 最 近 的 轨迹 点 ， 如 图 6.3 (a) 所 示 。 该 方法 
利用 道路 的 曲率 信息 ， 从 车 辆 质心 出 发 ， 在 局 部 作 垂 直 于 道路 的 直线 ， 直 线 
与 道路 的 交点 即 所 寻找 到 的 起 始 参考 点 。 经 过 多 次 仿真 实验 发 现 ， 该 方法 对 
于 尺寸 较 小 的 障碍 物 能 够 发 挥 作用 ， 无 人 驾驶 车 辆 可 以 在 参考 轨迹 点 的 指引 
下 顺利 达到 目标 点 ; 但 如 果 障 碍 物 尺寸 较 大 ， 如 杆 状 的 障碍 物 ， 车 辆 的 参考 
点 在 航 开 障碍 物 后 会 出 现 重 新 回 到 起 始点 的 情况 。 出 现 这 个 问题 的 原因 在 于 
没有 将 目标 点 信息 纳入 参考 轨迹 点 寻找 过 程 中 ， 当 车 辆 为 了 绕 开 障碍 物 而 规 
划 出 航向 角 超 过 90° 的 轨迹 时 ， 车 辆 便 会 将 已 经 走 过 的 参考 点 重新 作为 新 的 
参考 点 

为 解决 该 问题 ， 本 文 将 目标 点 信息 加 入 到 参考 轨迹 点 的 选择 过 程 中 。 车 辆 
在 跟踪 过 程 中 无 论处 于 任何 位 置 ， 都 会 在 全 局 坐标 系 下 作出 平行 于 x 轴 和 Y 轴 
的 两 条 直线 与 全 局 期 望 轨迹 相交 ， 如 图 6.3 (b) 所 示 。 由 于 始终 存在 两 个 交 
点 ， 故 分 别 计算 这 两 个 交点 与 日 标点 的 距离 ， 选 择 距 离 日 标点 较 近 的 参考 点 作 
为 真正 的 参考 起 始点 。 因此， 图 中 上 时刻 选择 点 2 作为 参考 点 ， 而 在 上 +1 时 
刻 选 择 点 3 作为 参考 点 


u P 
: A ES 目标 点 





4 se 
NN 1 Te kn 
e Ku 
p 
A 
起 始点 
(a) (b) 


16.3 寻找 参考 轨迹 点 的 方法 
(a) 方法 "3 (b) 方法 a 
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6.2.2 避 障 功能 函数 


在 无 人 驾驶 车 辆 行驶 环境 中 ， 障 碍 物 信 息 通 常 由 激光 雷达 的 障碍 物 点 给 
1 。 当 这 些 障碍 物 点 相互 靠近 时 ， 可 以 通过 聚 类 算法 将 它们 形成 
物 ， 而 障碍 物 点 相互 远离 时 ， 就 可 以 将 其 看 成 多 个 离散 的 小 障碍 物 。 因 此 ， 
障 功能 函数 可 以 针对 每 一 个 单独 的 障碍 物 进行 设计 。 惩 罚 函 数 的 基本 思路 是 
EN RENEE. ERUNT. GN 
大 。 综 合 车 辆 速度 与 目标 函数 中 惩罚 函数 的 比重 对 避让 障碍 物 的 影响 ， 选 择 如 
下 形式 的 避 障 功能 函数 : 

S us; 
fma tray vien sj (6.4) 

式 中 ，5,. 为 权重 系数 ，v; = が + が ,(x,、 y) 是 障碍 物 点 在 车 身 坐标 系 下 的 位 置 
(xo, y) 是 车 辆 质心 坐标 ,，* 为 较 小 的 正 数 ， 用 于 防止 出 现 分 母 为 0 的 現 
象 。 给 定 各 参数 后 ， 函 数值 随 障 碍 物 相对 坐标 变化 规律 如 图 6.4 (a) 所 示 。 

为 了 使 轨迹 规划 的 结果 实际 可 行 ， 需 要 根据 车 身 尺 寸 对 障碍 物 进行 膨胀 处 
理 。 本 和 有 文献 [1] PAB, 以 无 人 驾驶 车 辆 运动 中 心 的 内 切 圆 和 
外 接 圆 半径 为 尺寸 对 障碍 物 膨胀 。 当 障碍 物 尺 寸 较 大 时 ， 还 需要 按照 一 定 的 分 
辩 率 对 障碍 物 进 行 分 割 处 理 ， 防 止 出 现 车 辆 从 障碍 物 中 间 穿 越 的 现象 ， 如 
图 6.4 (b) 所 示 。 








图 6.4 和 仆 罚 函数 与 障碍 物 膨胀 
(a) 惩罚 函数 示意 ; (b) 障碍 物 膨胀 与 分 割 


将 上 述 避 障 功能 函数 代入 式 (6.4) rp, 设 权 重 系数 SUL 分别 为 10、100 
和 1 000， 给 定 不 同 初始 状态 后 的 规划 结果 如 图 6.5 所 示 。 当 规划 层 接 收 到 陪 
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碍 物 信 息 后 (图 中 点 1 和 点 2) ， 规 划 出 避 开 障碍 物 的 新 轨迹 ， 权 重 系数 的 增 
加 会 使 规划 结果 趋 于 保守 ; 而 当 车 辆 没有 接收 到 障碍 物 信 息 (图 中 点 3) 时 ， 
权重 系数 不 会 对 规划 结果 产生 任何 影响 。 当 车 辆 状态 测量 或 者 估计 存在 较 大 误 
差 时 ， 可 以 采用 较 大 的 权重 系数 ,但 同时 也 带 来 了 跟踪 偏差 增加 的 问题 。 





0 20 40 60 80 100 120 
X/m 


图 6.5 不 同 权 重 系数 下 的 规划 结果 


轨迹 重 规划 层 中 的 控制 目标 是 尽量 减少 与 全 局 参考 路 径 的 偏差 ， 并 且 实 现 
对 障碍 物 的 避让 。 对 障碍 物 的 避让 以 惩罚 函数 的 方式 实现 。 轨 迹 重 规划 层 的 模 
型 预测 控制 器 具体 形式 如 下 : 

min 2 ne ti -m« Gil) [at lU ls tJa; (65) 
st U,, SU ミミ の 
IUP, Ju AN EFEISE ZA] iOS REDE PR AC. 

由 于 规划 层 的 实时 性 要 求 比 控制 层 低 ， 点 质量 模型 相对 于 非 线 性 动力 学 模 
型 也 进行 了 较 大 程度 的 简化 ， 有 具备 更 高 求解 精度 的 非 线性 模型 预测 控制 算法 完 
全 能 够 满足 轨迹 重 规划 的 要 求 ， 而 非 线 性 日 标 防 数 的 采用 也 能 给 后 续 惩 罚 函 数 
的 设计 带 来 便利 。 因 此 ， 对 于 式 (6.5) 不 再 进行 线性 化 ， 而 是 直接 基于 非 线 
性 模型 求解 。 相 关 解 法 已 在 第 3 章 中 介绍 ， 此 处 不 再 重复 。 


6.2.3 S 次 多 项 式 轨迹 拟 合 
在 轨迹 重 规划 算法 中 ， 将 目标 函数 设 定 为 有 限时 域内 与 参考 点 的 距离 偏差 最 


max 


小 ， 所 规划 出 的 轨迹 是 以 预测 时 域内 离散 的 点 给 出 的 。 随 着 预测 时 域 的 增加 ， 这 
此 局 部 参考 轨迹 点 的 数量 也 会 随 之 增加 。 直 接 将 这 些 点 输入 控制 层 ， 占 用 了 过 多 


的 控制 器 输入 接口 ， 不 利于 控制 器 的 规范 化 设计 。 男 外 ， 由 于 规划 层 与 控制 层 的 
控制 周期 并 不 一 致 ， 控 制 层 很 难 根据 离 若 的 参考 轨迹 点 完成 轨迹 跟踪 的 任务 。 
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综合 上 述 因 素 ， 有 必要 对 轨迹 重 规划 算法 所 规划 出 的 局 部 参考 路 径 进 行 处 
理 ， 实 现 规 划 层 与 控制 层 的 顺利 对 接 。 曲 线 拟 合 是 对 离散 点 处 理 的 主要 方式 ， 
根据 所 采用 曲线 的 不 同 ， 有 样 条 曲线 拟 合 、 多 项 式 拟 合 以 及 考 指 数 拟 合 等 。 巾 
于 车 辆 存在 运动 学 约束 ， 如 车 辆 位 置 连续 要 求 曲 线 是 连续 的 ， 横 摆 角 连续 要 求 
曲线 是 一 阶 连 续 ， 而 加 速度 约束 则 要 求 曲 线 二 阶 连 续 。 因 此 ， 本 书 选择 5 次 多 
项 式 作为 拟 合 曲线 ， 形 式 如 下 : 


Y =at to a,b) ag) cata, 





sah à 3 2 (6.6) 
p = ba +b, +b, +b, + b,t +b; 


AP, a= [aos a,, Grs Oss Gay a5], b, = [bos Drs b, bos bas bs) AR 
求 参 数 。 

设 规划 层 预测 时 域 N, =16， 拟 合 结 果 如 图 6.6 所 示 。 图 6.6 中 ， 星 号 为 轨 
迹 重 规划 算法 在 预测 时 域内 规划 出 的 离散 点 ， 曲 线 为 拟 合 后 的 轨迹 曲线 

以 拟 合 后 各 数据 点 残 差 的 范 数 来 表征 拟 合 质量 ， 对 整个 轨迹 重 规 划 过 程 中 


0.5 


5 10 
采样 次 数 / 次 


(a) 


e C) 


0.02 
0 5 10 15 
采样 次 数 / 次 
(b) 
图 6.6 离散 轨迹 点 拟 合 结果 
(Qa) 横向 位 置 拟 合 结果 ; (Cb) Bst fit cres 
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的 201 次 规划 数据 进行 统计 ， 统 计 结果 如 图 6.7 所 示 。 从 图 中 可 以 看 出 ， 车 辆 
位 置 横 坐 标 工 的 拟 合 残 差 最 大 值 为 0.003 ， 车 体 航向 角 o 的 拟 合 残 差 最 大 值 不 
超过 7. 50 x10 ， 具 备 非常 高 的 拟 合 精度 。 


0.004 
0.003 
x 
な 0.002 
E 
0.001 
0.000 へ 
0 50 100 150 200 
重 规划 次 数 / 次 
(a) 
1.00X10* 
7.50Xx 10“ 
" 
你 
4z 5.00 x 107 
z 
EZ 
2.50X 105 
0.00 c 
0 50 100 150 200 
重 规划 次 数 /次 
(b) 


图 6.7 数据 点 拟 合 质量 统计 
(a) 憶 向 位 暫 的 扱 合 残 差 , Cb) 模 損 角 


6.2.4 非 线 性 二 次 规划 计算 

在 用 S 也 数 实现 基于 MPC 的 轨迹 规划 右 时 首先 震 要 确定 S 函数 的 输入 与 
输出 量 ， 如 图 6.8 所 示 。 基 于 MPC 的 轨迹 规划 器 将 CarSim 的 输出 作为 输入 ， 
CarSim 输出 的 参数 为 y. x. d. b, Y, X), AW AA EY BI X 坐标 方 
向 上 的 速度 、 航 向 角 及 航向 角速度 ， 在 了 和 六 坐标 轴 下 的 坐标 等 ， 共 6 个 参 
数 基于 MPC 的 轨迹 规划 器 的 输出 是 规划 好 的 期 望 轨 迹 方 程 的 参数 ， 因 为 选 
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hiemis cse de ce BC 
择 了 5 次 曲线 进行 曲线 拟 合 ， 所 以 这 里 的 输出 量 为 10 个 。 
规划 轨迹 前 轮 偏 


基于 MPC 的 方程 参数 基于 MPC 的 角 控 制 量 | CarSim 动 力学 
轨迹 规划 器 NE 轨迹 跟踪 控制 器 au. 仿真 模型 
CarSim 的 输出 作为 轨迹 规划 器 和 轨迹 跟踪 控制 器 的 输入 
图 6.8 各 个 模块 的 输入 与 输出 示意 


本 节 主 要 介绍 如 何 通过 Simulink 的 S - Function 设计 基于 MPC 的 轨迹 规 
划 器 。 读 者 直接 调用 Chapter6_ 2_ 4. m 文件 生成 S - Function 形式 的 轨迹 规 
划 器 。 

为 便于 代码 解析 ， 将 仿真 程序 文件 Chapter6_ 2_ 4. m 分 为 若干 模块 分 别 
介绍 。 程 序 后 面 有 备注 ， 以 方便 读者 阅读 。 

第 1 部 分 为 主 函 数 。 通 过 此 函数 完成 程序 的 初始 化 ， 并 设置 S 函数 模块 的 
输入 、 输 出 和 状态 变量 的 个 数 等 。sys 变量 在 不 同 的 阶段 表示 不 同 的 意义 。 





function[sys,x0,str,ts]-MPC TrajPlanner (t,x,u,flag) 
& 该 程序 功能 :用 点 质量 模型 设计 规划 期 ,能 够 规避 障碍 物 
% 程序 版 本 V1 .0 ,Matlab 版 本 :R2011a, 采 用 S 函数 的 标准 形式 ， 
$ 状态 量 = [y_aot ,x_qdot ,phi,,Y,X] ,控制 量 为 前 轮 偏 角 ay 
switch flag, 
case 0 $ flag -0 表示 处 于 初始 化 状态 ,此 时 用 函数 malIni- 
tializeSizes 进行 初始 化 
[sys,xO,str,ts]-mdlInitializeSizes;$ Initialization 
case 2 % flag =2 表示 此 时 要 计算 下 一 个 离散 状态 
sys -mdlUpdates (t,x,u);$ Update discrete states 
case3 $% flag=3 表示 此 时 要 计算 输出 
sys -mdlOutputs (t,x,u);$ Calculate outputs 
case (1,4,9) % Unused flags 
$ flag =1 表示 此 时 要 计算 连续 状态 的 微分 
$ flag=4 表示 此 时 要 计算 下 一 次 采样 的 时 间 ,主要 用 于 变 步 长 的 设置 
& flag=9 表示 此 时 系统 要 结束 
sys = []; $ 不 使 用 的 标志 量 置 空 


otherwise 
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error (['unhandled flag-',num2str (f1ag)]); 
$ Error handling 
end 
$ End of dsfunc. 
第 2 部 分 为 初始 化 子 函数 。 通 过 之 前 的 描述 可 知 ， 该 模块 有 5 个 离散 状态 
变量 ，6 个 输入 变量 ， 以 及 10 个 输出 变量 。 其 实现 过 程 如 下 所 示 : 





function[sys,xO0,str,ts]-mdlInitializeSizes 


sizes -simsizes; gs 用 于 设置 模块 参数 的 结构 体 

用 simsizes 来 生成 
sizes.NumContStates =0:* 模块 连续 状态 变量 的 个 数 
sizes.NumDiscStates =5:* 模块 离散 状态 变量 的 个 数 
sizes.NumOutputs -10;2 输出 期 望 的 Y 和 Phi 
sizes.NumInputs =6;% 模块 输入 变量 的 个 数 
sizes.DirFeedthrough -1;* 模块 是 否 存 在 直接 贯通 
sizes. NumSampleTimes -1;* 模块 的 采样 次 数 , 至少 是 一 个 
sys =simsizes (sizes); e 设置 完 后 赋 给 sys 输出 


x0 = [0.001;0.0001;0.0001;0.00001;0.00001;]; 
$ 状态 变量 设置 


str=[]; g 保留 参数 
ts =[0.10]; る 采样 周期 :这 里 轨迹 规划 的 周 


期 设 为 100 ms 
End of mdlInitializeSizes 


第 3 LY ACC CU s HU F RA 





function sys -mdlUpdates (t,x,u) 

SYS ニ X: を 更 新 状态 变量 

% End of mdlUpdate. 

第 4 KBAITE RI rss, 是 基 十 MPC 轨迹 规划 器 的 主体 。 





function sys -mdlOutputs (t,x,u) 
を uJ& CarSim 的 输出 ;t 是 时 间 变 量 ;x 是 状态 量 ,x = (y dot, x-dot 


phi ,Y,X] 
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ーーーーーーーー p 
Nx =5: s 状态 量 的 个 数 ， 
Np-15; s 预测 步 长 
Nc -2; s 控制 步 长 
Nobs -6; % 障碍 物 个 数 
Tap. ls $ Sample Time 
y_dot =u(1/3.6: を 速度 单位 是 km/h ,转换 为 m/s 
x dot-u(2)73.6 «0.0001;$ 単位 是 km% FEMA mÆ. 加 一 个 很 小 的 


数 可 以 防止 分 母 为 堆 


phi-u(3)*pi/180; s CarSim 输出 的 为 角度 ,角度 转换 为 弧度 

phi dot-u(4)*pi/180; $ 角速度 ,角度 转换 为 弧度 

Y-u(5); gs 单位 为 m 

X-u(6); $ 单位 为 m 

$ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニ 

* 参考 轨迹 生成 

$ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ー ニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ 

shape =2 .4: S 参数 名 称 , 用 于 参考 轨迹 生成 

dx1 =25 ;dx2 -21.95; & 只 是 参数 名 称 

dyl-4.05;dy2 -5.7; s 只 是 参数 名 称 

Xs1-27.19;Xs2-56.46; $s 参 数 名 称 

X phi-1:15 220; $ 这 个 点 的 区 间 是 根据 纵向 速度 (x_dot ) 
来 定 的 ， 


zl-shape/dxl*(X phi -Xs1)-shape/2; 

z2 -shape/dx2 *(X phi -Xs2)- shape /2 ; 

Y ref-dy1/2.*(1-*tanh(z1))-dy2 2. * (1 +tanh (z2)); 

$ phi ref -atan(dyl * (1. /cosh(z1)).^2 *(1 .2 /dx1)- dy2 * 
(L. /eosh (z2)).^2 « (1.2 /dx2)); 


$ = ニニ = ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ 
$ 和 矩阵 转换 PIRA AE EBEN AR SE ROM 
名 = ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ 


State Initial = zeros (Nx ,1 ) 
State Initial(1,1)-y. dot; 
State Initial (2,1)-x dot; 
State Initial (3 ,1 )=phi : 
State Initial (4 ,1 )=Y: 
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State Initial(5,1)-X; 
S 障碍 物 信息 设置 


X obstacle -zeros (Nobs,1); 

X obstacle(1:2)-30; 

X obstacle (3 :4 )=35: 

X obstacle(5:6)=32.5: 

Y obstacle = zeros (Nobs ,1 ) : 

Y obstacle(1)20.5; 

Y obstacle (2)-1; 

Y obstacle (3)-20.5; 

Y obstacle (4 )=1; 

Y obstacle(5)-20.5; 

Y obstacle(6)-21; 

Yref- (Y ref(1,round(State Initial(5,1))-«1:round (State 
Initial(5,1))«15)Y;$ Yref 采用 的 是 近似 算法 ,此 处 为 局 部 期 望 路 径 

Q-100*eye(Np,Np); 当 这 里 设置 评价 矩阵 ,都 设 为 了 1 .可 以 根据 跟 

蹊 情 况 加 以 调整 
R=20*eye(Nc,NC): る 
S =100: gs 避 障 南 数 的 松重 


mu-0.4; を 地 而 摩擦 系数 

gz9.87 

lb-[-mu*g;-mu*g]; 

ub-[mu*g;mu*g]; 当 设 置 约束 

A=[]; 

b= [J; 

Aeq = []; 

beq s Í]; 

options -optimset (‘Algorithm ; active - set"); 


[A,fval,exitflag]- fmincon (@ (x)MY. cost function (x, State. 
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Initial,Np,Nc,Nobs,T,Yref,Q,R,S,X obstacle,Y. obsta- 
cle),[0;0;],A,b, 
Aeq,beq,lb,ub,[],options);$ 求解 
fprintf ('exitflag-$ d Mn',exitflag); 


y. dot. predict = zeros (Np,1); € 以 下 根据 计算 出 的 控制 量 推导 所 有 
的 状态 量 
x dot, predict -zeros (Np,1); 
phi. predict -zeros (Np,1); 
Y predict -zeros (Np,1); 
X predict -zeros (Np,1); 
for is1:1:Np 
TT if i--Nc-1 
ay (1)-A (0); 
| y.dot predict(i,l)-State Initial(1,1)«T*ay 
| (i); $ 以 下 完成 状态 量 更 新 
x dot predict(i,1)-State Initial(2,1); 
phi. predict (i,1)-State Initial(3,1)«T*ay(i)/ 
State Initial (2,1); 
Y predict(i,1)-State Initial(4,1)«T * (State. 
Initial (2,1) * sin (State Ini- 
tia1 (3 ,1 ) )+ State Initial (1,1) 
* cos (State Initial (3 ,1 ) ) ) : 
X predict(i,1)-State Initial(5,1)«T * (State _ 
Initial (2,1) * cos (State Ini- 
tial(3,1))-State Initial (1,1) 
*sin(State Initial (3 ,1 ) ) ) : 
else 
ay (1)=A 2) ;$ 这 种 写法 是 仅仅 考虑 两 个 控制 周期 
y.dot predict(i,l)-y dot predict(i-1,1)«Te* 
ay (i); 
x dot predict(i,l)-State Initial(2,1); 
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phi, predict (1 ,1 )=phi_ predict (i-1,1)« T * ay (1 ) / 
x dot predict(i-1,1); 
Y predict (i,1)- Y, predict (i-1)« T* (State Ini- 
tial(2,1)*sin(phi predict (i- 
1))-y dot, predict (i-1) * cos 
(phi, predict (i-1))); 
X predict(i,1)-X predict (i-1)« T * (State Ini- 
tial(2,1)* cos (phi. predict (i- 
1))-y dot. predict (i-1) * sin 
(phi predict (i-1))); 
end 
end 
Paramater X Y-polyfit(X predict,Y predict,4); 
Paramater X PHI-polyfit (X predict,phi predict,4); 
OutPut (1 :5 )= Paramater X Y; 
OutPut (6 :10 )= Paramater X PHI; 
sys -OutPut; 
$ End of mdlOutputs. 


第 3 pA JR (Cfr eC DIE T RK 





function cost = MY, costfunction (x,State, Initial,Np,Nc, 
Nobs,T,,Yref,Q,R,S,Y obstacle)  $ 求 代价 
PKJ UD T PK ŽK 

cost -0; 

y dot -State Initial (1,1); 

x dot =State_TniEia1 (2,1 ) : 

phi-State Initial (3,1); 

Y-State Initial(4,1); 

X start -State Initial (5,1); 

y dot, predict = zeros (Np,1); 

x dot predict - zeros (Np,1); 

phi. predict - zeros (Np,1); 


Y predict - zeros (Np,1); 
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X predict -zeros (Np,1); 
Y error = zeros (Np,1); 
J obst -zeros (Np,1); 
ay - zeros (Np,1); 
for 1 エ =1 :1 Mp 
if i--Nc-1 
ay (i,1)2x(1); 
y.dot predict (i,1)-y dot-«T*ay(i,1); 
gs 以 下 完成 状态 量 更 新 
Xx dot, predict (i,1)-x dot; 
phi. predict (i,1)=phi +T *ay(i)/x dot; 
Y predict (i,1)-Y +T (x dot * sin (phi) y. dot * 
cos (phi)); 
X predict (i,1)-2 X start «T * (x dot * cos (phi )- y_ 
dot * sin(phi)); 
for j=1 :1 :Nobs 
J_obst (i,1)=J_obst (i,1)+1/ (((X predict (i,1))x 
.obstacle(j,1))^2 + (Y. predict (i, 
1)- Y obstacle (j, 1))^ 2 + 
0.000001); 
end 
else 
ay(i,l)ex Q); $ 这 种 写法 是 仅仅 考虑 两 个 控制 周期 
Y_Qdot_predict (i,1)-y dot, predict (i-1,1)« T * 
ay (1,1); 
X dot predict (i,1)-x dot; 
phi predict (i,1)-phi. predict (i-1,1)«T *ay (i x 
.dot predict (i-1,1); 
Y predict (i,1)- Y predict (i-1)« T * (x dot * sin 
(phi, predict (i-1))« y. dot. pre- 
dict (i-1)* cos (phi. predict ti- 
1)); 
X predict (i,1)- X predict (i-1)« T * (x dot * cos 
(phi, predict (i-1))- y. dot, pre- 
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dict (i-1)* sin (phi. predict (i- 
1335; 
for p-1:1:Nobs 
J obst (i,1)2J obst(i,1)-1/ (((X. predict (i, 
1))- X obstacle (p,1))^2 + (Y _ 
predict (i,1)-Y obstacle (p,1))^ 
2+0.000001 ); 
end 
end 
Y error(i,1)-Y predict (i,1)-Yref (i,1); 
b Yref 是 局 部 期 望 路 径 ,Y_ref 为 全 局 期 望 路 径 
end 
cost-cost-«Y error *Q*Y error«ay(1:2)*R*ay(1:2)4S * 
sum(J. obst (:)); 
$ End of CostFunction 449 


6.3 基于 MPC 的 路 径 跟 踪 控 制 器 


为 了 减少 计算 量 ， ie 里 选择 车 辆 的 线性 时 变 模型 进行 MPC 计算 。 模 型 状 
态 量 是 : (Y, X, ġ, 0, Y, X), an 量 为 车 辆 的 前 轮 偏 角 5。 为 保证 控制 的 
实时 性 ， 在 跟踪 控制 层 依 基于 线性 时 变 模型 的 模型 预测 控制 算法 ,设计 
如 下 形式 的 跟踪 控制 融 : 


min Y, nC + a eH D+ X AnG ei D Ha egg 
(6.7) 
s. AU < AU, < AU,. 
U. S AAU, +U, < U,, 
Jis: S Ye SS aeque 
Suede E < ) m < D ba mis te 
ヶ g »0 
xb. m [Yusas 9 | 为 轨迹 重 规划 层 输 出 的 局 部 参考 轨迹 。 


ef docs 
tef, local T rel local 


本 次 的 轨迹 跟踪 控制 句 是 基于 车 辆 简化 动力 学 模型 (小 角度 假设 ) 设计 
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的 , 在 第 5 章 有 详细 介绍 ， 这 里 不 再 袭 述 。 本 节 主 要 介绍 如 何 通过 Simulink 的 
S - Function 设计 基于 MPC 的 轨迹 跟踪 控制 器 。 可 直接 调用 Chapter6_ 3. m X 
件 生成 S - Function 形式 的 轨迹 规划 器 。 

为 便于 代码 解析 ， 将 该 仿真 程序 分 为 若干 模块 分 别 介绍 。 程 序 后 面 有 备 
注 ， 以 方便 读者 阅读 。 

Chapter6_ 3. m 第 1 部 分 为 主 函 数 。 


function[sys,x0,str,ts]-MPC Controller (t,x,u,f1lag) 
& 该 程序 功能 :用 LTV MPC 和 车 辆 简化 动力 学 模型 (小 角度 假设 ) 设 计 控制 
器 ,接受 来 自 规划 器 的 期 望 轨迹 ,实现 轨迹 跟踪 控制 功能 
gs 程序 版 本 V1 .0 ,Mat1ab 版 本 :R2011a, 采 用 S 函数 的 标准 形式 ， 
& 状态 量 = [y dot,x dot,phi,phi, dot, Y, X], f tŒ A fiti 4C (à fA 
delta f 
switch flag, 
case 0 
[sys,xO,str,ts]-mdlInitializeSizes;$ Initialization 
case 2 
sys -mdlUpdates (t,x,u);$ Update discrete states 
case 3 
sys -mdlOutputs (t,x,u);$ Calculate outputs 
case(1,4,9)$ Unused flags 
sys - []; 
otherwise 
error ([üànhandled flag = ',num2str(flag)]);$ Error 
handling 
end 
$ End of dsfunc. 


Chapte6_ 3.m 第 2 T4 Jg V] a E RR 





function[sys,x0,str,ts]-mdlInitializeSizes 
sizes-simsizes; 
sizes.NumContStates  -0;$ 


sizes.NumDiscStates  -6;$ 
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sizes.NumOutputs sig 

sizes.NumInputs = 直る 

sizes.DirFeedthrough -1;$ 

sizes.NumSampleTimes -1; 

SyS -simsizes (sizes); 

x0 = [0.001;0.0001;0.0001;0.00001;0.00001 ;0.00001]; 


global U; 

U- [0]; s 控制 量 初始 化 

SE デニ [5 る Set str to an empty matrix. 
ts - [0.02 0]; $ sample time:[period,offset] 


$ End of mdlInitializeSizes 


Chapter6 2 MPC Controller. m 第 3 部 分 为 更 新 离散 状态 量子 函数 : 


function sys -mdlUpdates (t,x,u) 
SYS =X; 


$ End of mdlUpdate. 
第 4 部 分 为 计算 输出 子 函数 ,是 基于 MPC 轨迹 规划 器 的 主体 。 





function sys =mdlOutputs (t,x,u) 


global a b; 

global U; 

Nx -6; s 状态 量 的 个 数 

Nu-1; e 控制 量 的 个 数 

Ny-2; S 输出 量 的 个 数 , 这 边 可 以 先 用 两 个 输出 量 
Np-25; を 预测 步 长 

Ncz1l0; & 控制 步 长 


Row=1000; $ 松弛 因子 
fprintf (‘Update start,t =% 6.3f\n’,t) 
y dot-u(1)/3.6;2 単位 是 km/h, 转 换 为 m/s 
x dot =u(2)/3.6+0.0001;% 后 面 加 一 个 非常 小 的 数 , 是 防止 出 现 分 
EE Jg f) hi OL 
phi -u()*3.141592654 /180;$ CarSim 输出 的 为 角度 ,角度 转换 为 
弧度 
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phi_dot =u (4) *3.141592654 /180 : 
Y=u(5);% 单位 为 m 
X=u(6): 单位 为 m 


$ ============================================================= 
根据 规划 器 的 输入 确定 参考 轨迹 

名 ーー ニー ニニ ニー ニー ニニ ニニ ニニ ニニ ニニ ニー ニニ ニー ニーー ニ ニー ニーーー ニ ーー ニニ ーー ニーー ニ ーー ニー ニー ニー ニニ ニー ニニ ニー ニニ ニニ ニニ ニニ ー 
Paramater X Y (1 ,1 )=u (7) ;$ 读 人 局 部 规划 器 的 数据 ,传递 进来 的 是 5 


次 曲线 的 参数 

Paramater X Y(1,2)-u (8); 
Paramater X Y(1,3)-u(9); 
Paramater X Y(1,4)-u (10); 
Paramater X Y(1,5)-u (11); 
Paramater X phi (] ,1 )=u (12); 
Paramater X phi (1,2)-u (13); 
Paramater X phi (1,3)-u (14); 
Paramater X phi (1,4)-u(15); 
Paramater X phi (1,5)=u (16); 


Sf =0.2:Sr=0.2: syms sf sr;$ 分 别 为 前 后 车 轮 的 滑 移 率 
1f-1.232;1r-1.468;$ syms lf lr;% 前 后 车 轮 距 离 车 辆 质心 的 距 
离 ,车辆 固有 参数 

Ccf =66900;Ccr =62700 ;Clf =66900 ;Clr = 62700:$ 分 别 为 前 后 车 
轮 的 纵横 向 侧 
偏 刚度 

m-1723;9-9.8;1-4175;$ m 为 车 辆 质量 ,9 为 重力 加 速度 ,I 为 车 辆 

绕 z 轴 的 转动 惯量 


X predict =zeros (Np,1);$ 用 于 保存 预测 时 域内 的 纵向 位 置信 息 , 是 
计算 期 望 轨迹 的 基础 
phi ref = zeros (Np,1);% 用 于 保存 预测 时 域内 的 期 望 轨迹 
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Y_ref =zeros (Np,1);% 用 于 保存 预测 时 域内 的 期 望 轨 迹 
dphi ref -zeros (Np,1); 


kesi = zeros (Nx-«Nu,l); 

kesi (1)=y_dot;% u (1)==X (1) 

kesi (2)2x dot;$ u (2 )==X (2) 

kesi (3 )=phi;% u (3 )==X (3) 

kesi (4)=phi_dot; 

kesi (5 )=Y: 

kesi (6)- X; 

kesi (7)=U (1);% 计算 kesi, 即 状态 量 与 控制 量 合 在 一 起 
delta £ =U (1); 

fprintf('Update start,u(1 )= 4.2£ \n’,U (1)) 
T-0.02; 仿真 步 长 

T a11-20;$ 临时 设 定 , 总 的 仿真 时 间 , 主 要 功能 是 防止 计算 期 望 轨迹 越界 


Q cell-cell(Np,Np);$ 权重 设置 
for iz1:1:Np:; 
for j5s131:Np; 
if $223 
Q_cell{i,j}= [2000 0;0 10000 ;]; 
else 
Q cellíi,j)-zeros (Ny,Ny); 
end 
end 
end 
R-5*10^5 eye (Nu * Nc); 
e 最 基本 也 最 重要 的 矩阵 ,是 控制 器 的 基础 ,采用 动力 学 模型 ,该 矩阵 与 车 
辆 参数 窜 切 相关 ,通过 对 动力 学 方程 求解 雅克 比 甜 阵 得 到 


a= [1- (259200 * T)/(1723 * x dot), - T * (phi dot + (2 * 
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((460218 * phi, dot)/5-62700 * y dot))/ (1723 * x dot^2)- 
(133800 + ((154 * phi. dot)/125 « y. dot))/ (1723 * x dot ^ 
2)),0, - T* (x dot -96228 / (8615 * x dot)),0,0 
T * (phi dot - (133800 * delta f)/ (1723 * x dot)), 
(133800 +*+ T * delta, f * ((154 * phi dot)/125 «y. dot))/ 
(1723 *x dot 2)41, 
0,T * (y dot - (824208 * delta f)/ (8615 * x dot)),0,0 
0,0 ,2.,T,9 ,0 
(33063689036759 æ T)/(7172595384320 * x _ dot), T æ 
(((2321344006605451863 * phi _ dot )/ 8589934592000 - 
(6325188028897689 * y dot)/34359738368)/ (4175 * x dot ^ 
2)+(5663914248162509 * ((154 * phi, dot)/425 « y. dot ))/ 
(143451907686400 * x dot^2)), 
0,1- (813165919007900927 * T)/ (7172595384320000 * x. 
dot),0,0 
T*cos(phi), T*sin(phi), T= (x dot * cos (phi)- y. dot x 
sin(phi)),0,1,0 
-T*sin(phi),T*cos(phi),- T* (y dot * cos (phi)« x dot * 
sin(phi)),0,0,1]; 

b= [133800 *T/1723;T * ((267600 * delta £)/1723 - (133800 x 

((154 + phi. dot)/125« y dot))/(1723 æ x. dot ));0; 
5663914248162509 * T/143451907686400 ;0 ;0]; 

d k-zeros (Nx,1);$ 计算 偏差 , 即 Ea1cone42 页 中 的 Q(k,t) 

state kl-zeros(Nx,1);$ 预测 的 下 一 时 刻 状态 量 , 用 于 计算 偏差 ， 

& 以 下 为 根据 离散 非 线 性 模型 预测 下 一 时 刻 状态 量 .gs 注意 ,为 避免 前 后 轴 
距 的 表 送 式 (a,b) 与 控制 器 的 a,b 和 矩阵 冲突 ,将 前 后 轴 距 的 表达 式 改 为 
lf 和 1r. 

state k1(1,1)2-y dot +T* (- x dot * phi. dot «2 * (Ccf * (de] - 
ta f - (y dot - 1£ * phi. dot)/x dot)«Ccr (lr 
*phi dot-y dot)/x dot)/m); 

State k1 (2,1)- x dot +T * (y dot * phi dot «2 * (Clf * Sf «Clr 
* Sr +Ccf * delta, f * (Gelta £- (y dot «phi . 
dot * 1£) /x dot)) AÁ); 

State kl (3,1)-phi +T * phi dot; 
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state kl (4,1)=phi_ dot +T ((2* 1f *Ccf * (delta f- (y dot + 
lf*phi dot)/x dot)-2 * lr*Ccr* (Ir*phi . 
dot -y. dot) /x dot) /T); 
state k1(5,1)2Y « T * (x dot * sin(phi)«y dot * cos (phi)); 
state k1(6,1)-2X «T * (x dot * cos (phi)-y dot * sin (phi)); 
d_k=state_k1 -a * kesi (1:6,1)-b * kesi(7,1); 
d_piao_k = zeros (Nx +Nu,1);% d_k 的 增 广 形式 
d piao k(1:6,1)-d k; 
d piao k(7,1)20; 
A cell-cell(2,2); 
B cell-cell(2,1); 
A cellí(1,1)-a; 
A cellí(1,2)-b; 
A cell(2,1)-zeros (Nu,Nx); 
A cell(2,2]-eye (Nu); 
B cellí1,1)-b; 
B cell(2,1]-eye (Nu); 
A-cell2mat (A cell); 
B-cell2mat (B cell); 
Cs LUT UCT LU OT 
PSI cell-cell(Np,1);$ 
THETA_cell = cell (Np,Nc); 
GAMMA, cell-cell(Np,Np);$ 
PHI cell-cell(Np,1);$ 
for p-l:l:Npbp:; 
PHI cellíp,1)-d piao k;$ 为 了 简便 ,这 里 认为 近似 相等 
for q-l:l3Np; 
if qespi 
GAMMA, cellíp,q)-C *A^(p -q); 
else 
GAMMA_cell {p,q}= zeros (Ny ,Nx + Nu) ; 
end 
end 


end 
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for j21:1:Np 
PSI cell(j;,1)2:C *A^j; 
for kz1:1:Nc 


if k«zj 
THETA cell(j,k)2 C * A^(j - Kk) * B; 
else 
THETA cellíj,k)-zeros (Ny,Nu); 
end 
end 
end 


PSI-cell2mat (PSI cell);$ size(PSI)- [Ny * Np Nx+ Nu] 
THETA = cell2mat (THETA, cell);$ size (THETA)- [Ny * Np Nu * 
Nc] 
GAMMA - cell2mat (GAMMA cell); 
PHI -cell2mat (PHI cell); 
Q-cell2mat (Q cell); 
H cell-cell (2,2); 
H_ce11 {1 ,1) THETA * Q * THETA +R; 
H cell(1,2)-2zeros (Nu * Nc ,1 ) : 
H. cell(2,1)-2zeros(l,Nu * NC) : 
H cell(2,2)-Row; 
H-2cell2meat (H cell); 
error 1 -zeros (Ny *Np,1); 
Yita ref cell-cell (Np,1); 
for p-1:1:ND 
ift«p*T»T all 
X DOT -x dot * cos (phi)- y. dot * sin (phi);$ 惯性 坐标 系 
F3 qn] ERE 
X predict (Np ,1 )=X+X_DOT *Np* T; 
Y ref (p,l)-polyval (Paramater X Y,X predict (Np,1)); 
phi. ref (p,1)- polyval (Paramater,. X, phi,X. predict 
(Np,1)); 
Yita ref cellíp,l)- [phi. ref (p,1) ;Y ref (p,1)]; 
else 
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X DOT -x dot * cos (phi)-y dot * sin (phi);$ 惯性 坐标 系 
下 纵向 速度 
X predict(p,1)2-X «X Dom*D*#Ti;g% 首先 计算 出 未 来 x 的 位 
置 ,X(t)=X+X_dot* 
E 
Y ref (p,l)-polyval (Paramater. X Y,X predict (p,1)); 
phi. ref (p,1)- polyval (Paramater, X phi,X, predict 
(m,ljys 
Yita ref cellíp,l)- [phi . ref (p,1) ;Y ref (p,1)]; 
end 
end 
Yita ref-cell2mat (Yita ref cell); 
error 1 -Yita ref -PSI * kesi -GAMMA * PHI ; $ 求 偏差 
£ cellazcell(iL,2); 
f cellí1,1)-2 * errof. 1 *Q* THETA; 
E geili, Jgs 
f= -cell2mat (f_cell); 


157 


A t-zeros(Nc,NC); 
for p-1:1:Nc$ % 以 下 为 约束 生成 区 域 
for ĝis Ll Në 


if q«-p 
A tip,d)-1; 
else 
A t (p,q)20; 
end 
end 
end 


A I-kron(A t,eye(Nu)); 

Ut = kron (ones (Nc,1),U (1)); 

umin- -0.1744;$ 维 数 与 控制 变量 的 个 数 相同 
uma メ =0.1744: 
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delta umin- -0.0148 *0.4; 
delta umax-0.0148 *0.4; 
Umin = kron (ones (Nc,1),umin); 
Umax - kron (ones (Nc,1),umax); 
ycmax- [0.21;5]; 
ycmin=[-0.3: -3];$ 输出 量 约束 ( 硬 约束 设置 ) ,此 处 让 所 有 输出 为 硬 
约束 
Ycmax = kron (ones (Np ,1 ) ,ycmax); 
Ycmin- kron (ones (Np,1),ycmin); 
S 结合 控制 量 约 束 和 输出 量 约束 
A cons, cell- (A I zeros (Nu * Nc,1); -A_I zeros (Nu * Nc,1); 
THETA zeros (Ny * Np,1); - THETA zeros (Ny * Np,1)); 
b. cons, cell = {Umax -Ut ; -Umin -Ut;Ycmax - PSI * kesi - GAMMA x 
PHI; -Ycmin «PSI * kesi + GAMMA * PHI}; 
A cons -cell2mat (A cons. cell);$ 状态 量 不 等 式 约 束 增益 矩阵 , 转 
换 为 绝对 值 的 取 值 范围 
| b cons =ce112mat (b cons cell);$ (求解 方程 ) 状 态 量 不 等 式 约束 
| 的 取 值 
| M=10: 
delta_Umin = kron (ones (Nc,1),delta umin); 
delta Umax = kron (ones (Nc,1),delta umax);$ 状态 量 约束 
1b = [delta Umin;0];$ (求解 方程 ) 状 态 量 下 界 , 包 含 控制 时 域内 控制 





增 量 和 松弛 因子 

ub = [delta Umax;M];$ (求解 方程 ) 状 态 量 上 界 ,包含 控制 时 域内 控制 
增 量 和 松弛 因子 

$$ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニニ ニー ニニ ニニ ニニ ニニ ニニ ニニ ニー ニニ ニニ ニニ ニニ ニニ ーー ニニ 

% 开始 求解 过 程 

% ニニ ニニ ニー ニテ ニー ニー ニー デー ニニ ニー ニニ ニニ ニー ニー ニニ ニー ニー ニニ ニニ ニニ ニー ニー ニニ ニニ ニー ニニ ニニ テー ニニ テー テニ ニニ ニニ ニニ ニニ ニー ニー ニー 


options -optimset ('Algorithm', active - set’); 

x start = zeros (Nc +1 ,1);% 加 入 一 个 起 始点 

[X, fval,exitflag]- quadprog (H, £,A. cons,b. cons, [], (1, 1b, 
ub,x start,options); 

fprintf('exitflag-$ d\n’,exitflag); 

fprintf(/(H-$ 4.2£ NXn',H(1,1)); 
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fprintf('f£2$ 4.2£Xn',£(1,1)); 


s 计算 输出 


u piao-X(1);$ 得 到 控制 增 量 
U(1)=kesi(7,1)+u_piao;% 当前 时 刻 的 控制 量 为 上 一 时 刻 控制 + 控制 


6.4 不 同 车 速 下 的 跟踪 控制 仿真 实例 验证 


设计 仿真 工 况 ， 测 试 轨迹 跟踪 控制 系统 在 不 同 速度 下 的 跟踪 能 力 。 依 然 以 
双 移 线 轨 迹 作为 车 辆 的 全 局 参考 轨迹 ,仿真 工 况 设 定 如 下 : 

轨迹 跟踪 仿真 实验 分 别 在 10 m/s, 20 m/s 和 30 m/s 的 速度 下 进行 ， 道 路 
附 着 条件 良好 、 =0.8。 参 考 轨迹 上 存在 一 个 障碍 物 ， 膨 胀 后 的 尺寸 为 5m x2 
m， 角 点 的 坐标 位 置 为 (30，0.5)。 上 层 规划 器 所 采用 的 参数 为 : 7=0.1 s, 
V 215, N, 22, Q=10, R=5，5,,. =10。 下 层 跟 踪 控制 器 所 采用 的 参数 为 : 
T=0.05 s, N, 225, N, 210, -10*«5,«10^, -0.85°<Aô, <0. 85°, IEH 


阵 设置 为 : 


200 0 0 
e-o 100 o || R =1.1 x10, p = 1000, 
0 0 100 


下 面 介绍 CarSim/Simulink 进行 联合 仿真 的 具体 步骤 : 

首先 要 新 建 一 组 Datasets, 出 现 如 图 6. 9 所 示 的 对 话 框 。 在 两 个 文本 框 中 分 
别 输入 “Example” 和 “MPCTracker”， 然 后 点 击 “Set"”， 完 成 新 建 。 此 时 ， 选 
择 主 菜单 中 的 “Datasets” 下 拉 菜 单 ， 发 现 “Example” 一 栏 中 多 出 “MPC- 
Tracker” 


Category. F «ample [v] |  Tte.]|MPCTreckad 


The current category and title for this data set ere shewn in the window úte bar The screenwll | Set blo Change. 
be locked except for the controls in this panel unti yoa click a button (Set or No Chenge) ーー 


[436.9 新婦 Dataset 
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然后 进入 模型 编辑 界面 ， 如 图 6.10 所 示 。 








H a? 


Test ÓÀ Run Control with Simulink Results (Post Processing) 





二 ーー bi Run Now. Send to Simulink "^-^ 7 ' 
UA F 
bid ト sq Annum rw he l- 
z Wrte dii aatia atoh SET 











CarSim 


Mechanical Simulation 





[46.10 CarSim 主 界面 


NEUE TU NRNEE E LE, 、 数 学 模型 求解 以 及 后 处 理 。 下 
进行 详细 介 


6.4.1 车 辆 参数 设置 

在 车 辆 参数 设置 部 分 ， 需要 设置 的 车 辆 参数 有 整 车 尺寸 和 外 形 、 系 统 和 前 
后 悬 架 等 ， 如 图 6. 11 所 示 。 

按照 德国 汽车 分 级 标准 ，A 级 (包括 A, A0, A00) 车 是 指 小 型 轿车 ，B 
级 车 是 中 档 轿 车 ，C 级 车 是 高 档 轿 车 ， 而 D 级 车 指 的 则 是 豪华 轿车 。 其 等 级 划 
分 主要 依据 轴 距 、 排 量 、 重 量 等 参数 ， 字 母 顺序 越 靠 后 ， 该 级 别 车 的 轴 距 越 
长 ， 排 量 和 重量 越 大 ， 轿 车 的 豪华 程度 也 不 断 提 高 

这 里 我 们 以 比亚迪 速 锐 汽车 作为 原型 进行 建 模 。 速 锐 排 量 1.5 L, AiE 
2.66 m, ^E 5E HE 4.68 m。 这 里 在 C 级 车 的 基础 上 进行 修改 。 点 击 图 6.12 中 
的 下 三 角 ， 选 择 相应 的 车 型 。 这 里 选择 “CS C - Class, Hatchback” — 车 型 


6.4.2 ”仿真 工 况 设置 
首先 进入 仿 直 工 况 设置 界面 ， 如 图 6. 13 所 示 
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O- 











图 6.11 车 辆 参数 设置 内 容 


Lcd q^ ob Xy DL og 
(1) 仿真 工 况 参数 设置 Test Specifications 


占 idi "New", 37 4E Dj E TA, 在 图 Vehicle configuration. Ind. Ind 了 | 


6. 14 所 示 的 文本 框 中 依次 输入 “MPC Ex- 
ample” 和 “MPC ENV”, Jil "Set", 图 6.12 ”选择 车 型 


i t, 





"uy ns X Wo? 
i "hs P - E deb» Habes nee do 
Driver Controls Start and Stop Conditions Plot Definitions 


RS V Sop my dpe x Lva B= ta Pus (ut) Maston ト 
Yo < 3 tator wr 

oo -E 3 EEEE .. ]- 
M lcu ーー em Rondkmwe v Broer — Ro sd Wheel Emer (Cer, Fronty ト 

ob od ト ーー 
ect tahran teet 4kha ip^rges Con AU) ト 
ニー こと " ん ataAoteooe に 

AT in Mode ] 

Fot -Spe Masses ト 
tein Cosrlocp viver Caraot . 7 = " 
ene |l Additional Data an Peta — Serung Mar ] 
Ms - Fi- Vwrscal Fercns (Cm AN ト 




















Additional Data 
Mec. Diwedd 


1 Am Sqreit pAr = 146 ト 


























[46.13 仿真 工 况 的 主 界面 


Cat ory MPE Example Y Tie jMPCENM 


he curent category end tlle Tor this data set are shown in the window le tier The screen will Sot | NoCheange | 


JE lucked excep! lor he Controls in irs panel until you click e button (Set or No Change) 


(6.14 JA vc rh 
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完成 新 建 。 
根据 要 求 设置 仿真 工 况 : 
(D 目标 车 速 为 18 km/h, 
© 无 制 动 。 
③ 挡 位 控制 选用 闭环 AT 5 挡 模 式 。 
④ 无 转向 控制 ， 方 向 盘 转角 为 0 deg. 
(5) 路 面 选择 为 1 km ， 摩 擦 系数 为 1.0 的 方 形 路面 。 
(6 完成 设置 后 ， 如 图 6. 15 所 示 。 
(2) 设置 仿真 时 间 
同样 在 设置 仿真 工 况 的 主 界面 ， 在 如 图 6. 16 所 示 的 文本 框 内 输入 “100”。 
设置 完成 后 ， 点 击 “Save”， 进 行 保存 。 


Driver Controls 


Constantiargetspeed vw 18 kw 





Start and Stop Conditions 
| Slop run at speofied lime X 
Time (ec) Road strion (m) 
Start 0 o 
Stop 100 Road toward wi 





图 6.15 仿真 工 况 设置 图 6.16 仿真 时 间 设 置 


(3) 设置 仿真 步 长 

在 CarSim ŁA MP, Hi FMA “Tools”, yF% “Preference”, HINUN 
图 6. 17 所 示 的 界面 ， 将 仿真 步 长 设置 为 “0. 001”。 

点 击 “Home” 图 标 ， 返 回 CarSim 的 主 界面 ， 在 “Procedure” 项 日 下 选择 
前 面 新 建 的 “MPC ENV” 工 况 ， 完 成 仿真 工 况 设置 ， 如 图 6. 18 所 示 。 
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Defaull Settings lor Math Models 


eleu me stap "tervals tortha math modals end output files used 1ro oher 
volves ere p "ouded tci a run 


Timc sep ‘sec) Freq (Hz) 
Mat- medel u w | yu 
Outputilz 0025 4l 








[6.17 设置 仿真 步 长 图 6.18 新 建 仿真 工 况 


6.4.3 CarSim/Simulink 联合 求解 


(1) 建立 CarSim/Simulink 的 关联 
点 击 如 图 6. 19 所 示 的 Models 选项 ， 选 择 “Models: Simulink” , 
点 击 如 图 6. 20 所 示 的 选项 ， 选择“ [Link to New Dataset]" 。 











Run Control with Simulink Run Control: Builtdn Solvers 
imulink | Modes. vw E- "[Madels: - 
Bun Now nd Run Math Model [Meds = 
。 [Ne linked library) 
| Models: Self-Contained Sovers | Nodgeeeiselpced ト 
Y Models. Simufink | - —— ——— 
Modes: ransfer io Local Windows Deectcry Y INO Dataset ooo — 163 
Modelc: Trancfer to RT-Lab Tarqet | pink ta New Dataset] z] 
Modelc Transfer to dSPACE Target ABS Cortrol'er 
Models Transiter to Target by FTP Act've Suspension 
Models ETAS aSCET Brake Cont. with Moving Objects (2 Sensors) 


Model ' Wc ; 
Models: LabVIEW fo: V nzows (VI) Brake Cont with Moving Objects (CS8) 


Modeic: Transfer to LibVIEV &7 Target n . v 
Rirske Central with Moving Objart 


[436.19 选择 Simulink 接口 [56.20 Link to New Dataset 


此 时 将 弹出 一 个 如 图 6.21 所 示 的 对 话 框 。 在 对 话 框 中 依次 输入 “Exam- 
ple” 和 “MPC_ Simulink”， 点 击 “Create and Link” 完 成 新 建 。 


-二 Unkto New Dataset DERE E 





Category for new dataset 
Example (v) | 


Create and Link Cancel 


图 6.21 新 建 一 个 与 Simulink 联合 的 Dataset 
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创建 完 新 的 Dataset 之 后 ， 将 其 与 Simulink 关联 起 来 ， 如 图 6. 22 所 示 。 
在 将 CarSim 的 模型 关联 到 Simulink 之 前 ， 需 要 先 新 建 一 个 Smulink 文件 
并 保存 。 点 击 如 图 6.23 所 示 的 “MPC_ Simulink”， 将 会 弹出 如 图 6. 24 所 示 的 
Simulink | Example] MPC_ Simulink 的 主 界面 。 
Run Control with Simulink 


Bun Now Send to Simulink Muse ~ 





[No Dateset] 

ICogy end Link Detasei] 

ABS Controller 

Active Suvpenion 

Brake Cont. with Moving Objects (2 Sensors) 
Brake Cont with Moving Objets (CS8) 
Brake Control with Moving Object 

Custom Steer S-Functon 


Drver Model (C58) 

Cmeme 590035 

Four Wheel Steering System 

Mitch Contrsller 

Type ^ ABS Controller 

Type B ABS Controlla- 

OE eas : Run Control with Simulink 
KE 75 as Bun Now Send to Simulink — "os て 

Eeema Sowertran ^ v MpCtestl 
DE I 

图 6.22 选择 新 建 的 “MPCtestl” dataset 图 6.23 点 击 “MPC_ Simulink” 
ee le ee um x "E" 





B A?i 


tes ーーー - i wm Fete ex So 
Dom TA 
ーー II EE )1 NEY SND 
Yeusen unio rains hue ja ed Wat deca» ve orem ihi iste We fe winien OS Une él ba Mats vi 
タク トー の yo We 
v — pochi manevs server DUL ee 
See tmo Pm 


Uis We held bow lo diia cr comandi scrocaivd ed fri sod dara re 


gor etos AM i Lupaan pe wap m 
Send. iom inp = 1/3 Cam we dp 
ir 


[86.24 MPC_ Simulink 的 主 界面 
选择 相关 路 径 。 这 里 的 工作 路 径 和 Simulink Model 的 路 径 是 指 Simulink X 


件 保存 的 路 径 。 可 以 根据 实际 情况 选择 “Use 64 - bit matlab" 或 者 “Use 32 - 
bit matlab”， 如 图 6. 25 所 示 。 
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hr Musee inmw ope Er A MPa Simulink Hodal Cerdo Lese nap pomo- PaRI EAR e ge p ta fe Sand. | 





Uv aur stor refresh "st LH | Fora Wade DS] Useb4btMatab Y 


pec ly atemat v5 soteer DLL files 


[56.25  CarSim 模型 导入 Simulink 路 径 设置 


(2) 定义 CarSim 的 导 人 变量 

点 击 如 图 6. 26 (a) 所 示 的 “Import Channels”， 选 择 “1/O Channels; Im- 
port". 然后 点 击 如 图 6.26 (b) 所 示 的 “Copy and Link Dataset”， 出 现 如 图 
6.26 (c) 所 示 的 对 话 杠 ,输入 “MPC_ Simulink Input”。 


Import Chennels ー [No Dataset] 


[Copy and Link Dataset] 
ABS input 





[No linked library] 
External PARSFILE 





Generic Data Group Driver Import 
iv 


Generic Data Lists 





. : Hitch Controller 
Generic V$ Commands m — 


Y [/O Channels: Import に ニー —— v MPCtest1 input 





zv Link to New Dataset let 


| 
| Category tor new dataset 








Title tor new dataset | | 
MPCtestl inpul | 


Create and Link | | Cancel | 


Cc) 


[86.26 新 建 MPC_ Simulink Input 


点 击 “MPC_ Simulink Input”， 显 示 如 图 6. 27 所 示 的 界面 。 
这 里 需要 通过 浏览 找到 “Readme file imports:”， 其 后 显示 为 : 


Programs V solvers \ ReadMe Vf. i_ i_ _ s_ imports | tab. txt 
定义 CarSim 的 导入 变量 为 车 轮 的 前 轮 偏 角 ， 顺 序 依次 为 IMP_ STEER 
LI (左前 轮转 角 「 deg | ) IMP. STEER_ RI (右前 轮转 角 [deg]), IMP. 


STEER_ L2 ( 左 后 轮转 角 [deg]), 以 及 IMP_ STEER_ R2. ( 右 后 轮转 角 

deg]) 。 需 要 注意 的 是 :CarSim 的 导入 变量 和 Simulink 中 MPC 模型 的 输出 量 
是 相对 应 的 ， 所 以 这 里 的 CarSim 的 导入 变量 应 按照 如 图 6. 28 所 示 的 顺序 排列 
选择 
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一 一 和 





2273 


4 | vt S e 
Tou car wife ro'es wie V fro | =] f> Betta. me Gowan, 


























me Variables Avalablo tor import — Artro impor! Variables 
———— —— —49 r Mos Mame Moca vavan 
-> ‘tec 
| | ae 
| pon 
| nook] 
| MP | 
Tos Qear 
| le 
一 Cr ma 
Achat door varat ies can te deed ring ia Sao ace か ef esoe a vent 
saioa mat you prov hie The syntas i ー 
EQ, IN «ame ~ pon 
| where «nimm is the rame of fà ave mpor verbis and 
eot var aktra ef デー roten math moo 
vaatis OM Venda Sm comm ads can be used Yo detre 
Boden, anatiec 
= っ 
> 
A 
图 6.27 导入 变量 设置 界面 
Feeadro fll “or imports: Frog masoers Isade. .] Relesh | | View zpreacsneet 
Categories Active Import Varlables 
SeeczyMpeolcormporent — — Move Harme Mode — Vahal'Vaoe 
AercdineTwics selectect i mp sirsa ii AN . 20 
Amikor vanas z3 sw — 2 Wp i Ad |-| so 
Erake [HPF F. BOOST. 1 mer ? vP Ffa i Nxt 0 
Caria [NP ae € WE EA AM 0 
(hasss metn M M BOOST 1 "m uu 
Dnvor modal |IMP-M-EJOST.Z 
Driver p wew pe nts Wl r 
Exerc RED IHP M.r7 2 
: | YY 
| [IMP SECR coN i | M 
IMP. STFFR, CON. 2 | om 
| [INP STEER cow n! | es 
IMP STEER CON, R? | ーー 
ーー ーーーーー イ 4 上 上 NESTEBR.RS 
Suspenena | | IMECSTEER RACK CON LI | 
Tues IMP STEER RACK CON L2 | 
Whaat IHP STEER HACK, CON F* i 


[IMP STEER RACK CON F2 
[IMP Hw 

.| UMP STEEP. " IN 
—— 0 actiesta a var anis 





图 6.28 定义 CarSim 导入 变量 


(3) 定义 CarSim 的 导出 变量 

步骤 与 本 节 (2) 定义 CarSim 导入 变量 相同 ， 首 先 新 建 名 为 “MPC_ Simu- 
link Output” 的 一 个 Dataset。 点 击 “MPC_ Simulink Output" ， 弹 出 如 图 6. 29 所 
示 的 界面 。 

通过 浏览 找到 Readme file for outputs， 其 后 显示 为 : 

Programs V solvers \ ReadMe V i_ i_ outputs tab. txt 


定义 CarSim 的 导出 变量 依次 为 VyBf_ SM (质心 处 的 纵向 车 速 [ km/h 1) 
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| -6amash | VviwSpreedsheet 
一 一 Available Vanablos ^ro Long Names Active Vanablos for Export 
Lii — | g Meve — 1L Vel SM S 
sseced < Df SM 
LI a 
Jpi3ewn £ AY 
in me let 





Dcutle-cick o^ a a^ 3ble rame nere to achane it Douzkedick or a varate rame here to Goa-trvare if 


[86.29 定义 CarSim 导出 变量 


VxBf_ SM, Yaw ( 偏 航 角 [deg]), AV_ Y, YO (坐标 系 立轴 的 坐标 值 
(mj) 和 X0 (坐标 系 X 轴 的 坐标 值 [m] ) 。 需 要 注意 的 是 : CarSim 的 导出 变 
量 和 Simulink 中 MPC 模型 的 输入 量 是 相对 应 的 ， 所 以 这 里 的 CarSim 的 导出 变 
量 应 按照 如 图 6. 30 所 示 的 顺序 排列 选择 。 

(4) 将 CarSim 建立 的 车 辆 模型 输出 到 Simulink 

点 击 “Home”， 返 回 CarSim 的 主 界面 。 点 击 “Send to Simulink”， 出 现 如 
[]6. 31 所 示 的 界面 。 此 时 Matlab 及 之 前 新 建 的 空白 模型 “Chapter6 _ 4_ 
3. mdl” 将 被 打开 。 


Active Variables for Export 


1 VyBf SM NNI 

2 VxBf SM Run Control with Simulink 

3 Yaw Run Now Send to Simulink Medes て 

SAY - 

5. Yo Wata all ava lable outputs 

6 Xo | Uutput voncbles マ 
[46.30 CarSim 的 导出 变量 图 6.31 数 学 模型 求 解 益 


如 图 6. 32 所 示 、 在 Matlab Command Window 中 输入 “simulink”， 回 车 ， 
打开 “Simulink Library Browser”, 注意: 现在 的 Simulink Library Browser 比 单 
IA. Matlab/Simulink 时 多 了 一 个 “CarSim S - Function”， 如 图 6. 33 所 示 

将 “CarSim S - Function” 拖 电 到 之 前 新 建 的 simulink 文件 中 。 注 意 此 模块 
恰好 有 一 个 输入 接口 和 一 个 输出 接口 ， 分 别 对 应 CarSim 的 导入 变 量 和 导出 变 最 ， 

完成 上 述 设 定 后 ，CarSim 通过 外 部 接口 将 车 辆 模型 发 送 至 指定 路 径 下 的 
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4& MATLAB Command Window |. 2-6 d 





lo get started, type one of these: helpwin, helpdesk, or demo. 
For product information, visit wwu.mathworks.com. 


> Warning: Calling MEX-File ‘D:\Program FilesNVCarSin802 .ProgXProgramsVSolversVes sf.dll'. 
ポー files with .dll extensions vill not execute in a future uersion of MATLAB. 


[56.32 Matlab Command Window 命令 窗口 











" ^ mes 
Wi Simulink Library Browser (= | © jes 
Fle Edit View Help 
| D gy » Entersearchterm -H [3] 
Libraries Library CarSim S-Function Search Resuks (none) Most Freqi * fb | 
"1 
e Simulnk ajj 
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图 6.33 含有 CarSim S - Function 模块 的 Simulink Library Browser 


Simulink 仿真 文件 中 ，CarSim 模块 即 以 S$ — Function 的 形式 增加 到 Simulink £i 
型 库 中 。 

(5) S 函数 与 Simulink 环境 下 的 仿真 

在 Chapter6_ 4_ 3. mdl 中 加 入 基于 MPC 的 轨迹 跟踪 控制 器 ，CarSim 的 导 
出 量 经 过 该 控制 器 的 计算 ， 决 策 出 下 一 时 刻 的 质心 车 速 和 前 轮 偏 角 ， 然 后 导入 
到 CarSim 模块 里 。 

将 Simulink Library Browser 中 的 “S - Function” 拖 电 到 simulink 文件 中 ， 
双击 该 图 标 ， 弹 出 如 图 6.34 所 示 的 对 话 框 。 在 “S - function name” 中 输入 
“chapter6_ 2_ 4. m”"。 点 击 “OK"， 将 设计 好 的 基于 MPC 的 轨迹 跟踪 控制 算 
法 的 m 文件 导入 simulink 文件 。 注 意 此 模块 恰好 有 一 个 输入 接口 和 一 个 输出 接 
口 ， 分 别 对 应 着 轨迹 规划 器 (chapter6_ 2_ 4.m) 的 导入 变量 和 导出 变量 

同 理 ， 将 基于 MPC 的 路 径 跟 踪 控 制 器 也 导入 Simulink 中 。 由 于 车 辆 的 姿 
态 变化 有 一 点 延 时 ， 所 以 在 Simulink/CarSim 联合 仿真 平台 中 加 入 延 时 模块 ， 
如 图 6. 35 所 示 。 

仿真 结果 如 图 6. 36 所 示 。 从 图 中 可 以 看 出 , 在 3 种 不 同 速度 下 规划 与 控 
制 系统 都 顺利 完成 了 对 障碍 物 的 规避 以 及 全 局 参考 轨迹 的 跟踪 。 图 6. 36 (a) 
中 ， 车 辆 从 原点 位 置 出 发 ， 此 时 并 没有 感知 到 障碍 物 的 存在 ， 重 规划 的 轨迹 与 
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[46.35  Simulink/CarSim 联合 仿真 平台 


24 brin 当 车 辆 行驶 至 20 m 处 时 ， 障 食物 信 息 的 加 入 使 得 重 规划 的 轨 
迹 伪 离 了 参考 轨迹 ， 通 过 一 系列 的 重 规 划 轨 迹 ， 无 人 驾驶 车 辆 实现 了 对 障碍 物 
的 规避 ， mem 上 参考 轨迹 并 保持 稳定 。 图 6.36 (b) 与 图 6.36 (e) 中 ， 
由 于 和 车速 的 增加 ， 相 同 预测 时 域内 规划 出 的 轨迹 区 间 也 相应 增加 ， 因 此 车 辆 在 
出 发 时 就 能 够 规划 出 绕 开 障 但 物 的 局 部 轨迹 ， 实 现在 高 速 下 的 障 但 物 规避 与 轨 
迹 跟 路 

本 音量 針 対 第 4 意 中 所 提出 的 轨迹 跟踪 控制 系统 的 不 足 而 进行 的 补充 与 完 
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(c) 
图 6.36 仿真 工 况 1 的 仿真 结果 
(a) 10 m/s 仿真 结果 ; (b) 20 m/s 仿真 结果 ; (e) 30 m/s 仿 直 结果 


善 ,在 原 有 的 控制 层 上 加 入 规划 层 ， 形 成 “轨迹 重 规划 + 跟踪 控制 ”的 双 层 控 
制 体系 。 在 轨迹 重 规划 算法 中 ,结合 非 线 性 模型 预测 算法 与 避 障 功能 函数 ， 实 
现 对 障碍 物 的 可 靠 规避 。 对 参考 轨迹 上 轨迹 点 的 选取 方法 进行 了 改进 ， 提 出 了 
一 种 融合 目标 点 信息 的 方法 ,防止 出 现 车 辆 倒退 的 现象 。 为 了 简化 规划 层 与 控 
制 层 之 间 的 传递 参数 ， 提 出 了 一 种 基于 5 次 多 项 式 曲 线 拟 合 的 方法 ， 将 规划 出 
的 轨迹 点 进行 拟 合 。 统 计 结 果 表明 ， 这 种 方法 具有 很 高 的 精度 。 最 后 ， 在 两 种 
不 同 的 工 癌 下 进行 仿真 实验 。 结 果 表 明 ， 所 设计 的 双 层 控制 体系 能 够 在 不 同 速 
度 、 不 同 障碍 物 环境 中 实现 对 参考 轨迹 的 可 靠 、 稳 定 跟踪 。 
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航向 跟踪 预 估 控 制 算法 


模型 预测 控制 虽然 能 够 很 好 地 表达 运动 规划 和 跟踪 控制 过 程 中 模型 约束 问 
题 ， 但 实际 应 用 时 ， ae & 耗 时 而 无 法 在 蔡 人 式 系统 中 实现 。 尽 管 
如 此 ， 模 型 预测 控制 思想 还 是 可 以 在 无 人 驾驶 车 辆 控制 过 程 中 得 到 实际 应 用 。 
本 曹 介 绍 了 著者 在 实际 车 辆 控制 中 运用 的 单 步 航向 预 估 控 制 方法 ， 虽 然 没 有 进 
行 优化 ， 但 在 考虑 模型 约束 的 基础 上 ， 结 合 PID 控制 方法 ， 在 单片机 程序 中 实 
现 了 无 人 驾驶 车 辆 航向 单 步 预 测 ， 取 得 了 较 好 的 控制 效果 デ 


7.1 H 述 


航向 跟踪 是 无 人 驾驶 车 辆 路 径 跟踪 的 一 种 基本 方法 。 在 该 方法 中 ， 位 置 偏 
ui 航向 偏差 最 后 都 可 以 被 转化 为 航向 偏差 。 因 此 ， 精 确 的 航向 跟踪 是 实现 路 
£ 趴 中 的 前 提 条 件 。 无 人 驾驶 车 辆 是 一 个 有 较 大 延迟 的 高 度 非 线 性 的 复杂 系 
统 ， 建 立 精 确 的 数学 模型 十 分 困难 ， 在 进行 航向 跟踪 控制 时 ， 参 数 的 变化 对 系 
统 异 型 影响 较 大 ， 其 中 纵向 速度 的 变化 影响 最 为 明显 。 无 人 驾驶 车 辆 航向 跟 足 
般 控 制 方法 是 把 期 望 航 癌 与 车 辆 实测 航向 之 差 作 为 控制 器 输入 偏差 ， 而 控制 
融 输 出 控制 量 则 为 机 费 人 的 前 轮 仿 角 
无 人 驾 驯 车 辆 的 航向 与 其 纵 癌 速度 、 横 加 速 度 、 前 轮 俩 用 、 车 辆 绕 其 重心 
的 转动 惯量 、 重 心 位 置 、 前 后 轮 侧 偏 系数 以 及 实际 道路 情况 等 诸多 因素 有 关 。 
在 常规 控制 方法 中 ， 只 考虑 了 期 望 骸 癌 与 实际 航向 的 偏差 ， 而 未 能 包含 其 他 因 
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素 的 影响 ， 因 此 难以 达到 满意 的 控制 效果 。 当 系统 参数 ， 特 别 是 某 些 敏感 参数 
发 生变 化 时 ， 就 必须 重新 设 定 控制 局 参数 。 例 如 ， 用 常规 PID 控制 希 进 行 航 回 
跟踪 实验 ， 在 某 一 纵向 速度 下 整定 好 PID 控制 参数 ， 而 当 纵 向 速度 发 生 很 小 变 
化 时 ， 就 必须 重新 整定 PID 参数 ， 否 则 控制 性 能 变 坏 ， 超 调 较 大 ， 甚 至 出 现 振 
荡 。 表 现在 路 径 跟踪 实验 中 ， 则 是 在 一 定 速度 下 能 较 好 地 完成 弯 道 或 急 灾 等 路 
径 跟 踪 任 务 ， 而 速度 变化 后 ， 跟 踪 误 差 变 大 或 出 现 大 幅度 振荡 。 因 此 ， 在 无 人 
驾驶 车 辆 航向 跟踪 控制 中 ， 控 制 方法 应 该 能 对 纵向 速度 等 影响 因素 有 一 定 的 目 
适应 能 力 。 

以 下 首先 介绍 简化 的 无 人 驾驶 车 辆 的 二 自由 度 动 力学 模型 ， 以 便 对 提出 的 
方法 进行 仿真 分 析 ， 然 后 对 航向 跟踪 预 估 控 制 算 法 的 基本 原理 进行 说 明 ， 最 后 
给 出 了 仿真 与 实验 结果 。 


7.2 二 自由 度 无 人 驾驶 车 辆 动力 学 模型 


无 人 驾驶 车 辆 有 纵向 、 横 向 和 垂直 方向 的 平 动 ， 以 及 侧 倾 、 俯 仰 和 横 控 3 
个 方向 的 转动 。 其 中 ,横向 运动 和 横 摆 运动 基本 上 是 由 转向 操纵 而 产生 的 两 种 
运动 。 当 横向 加 速度 和 横 摆 角速度 较 小 时 ， 常 采用 经 简化 的 二 自由 度 无 人 驾驶 
车 辆 单轨 模型 ， 如 图 2. 5 所 示 。 将 无 人 驾驶 车 辆 转向 机 构 视 为 一 阶 惯性 环节 ， 
则 有 : 





T +8, = 8; (7.1) 
其 中 ，r 为 惯性 时 间 常 数 ; 5, 为 期 望 前 轮 偏 角 ， 即 控制 器 输出 的 控制 量 
在 如 图 7. 1 所 示 的 无 人 驾驶 车 辆 路 径 跟 踪 示 意图 中 ， 为 期 望 路 径 上 的 预 
S, o ,为 航向 偏差 角 。 假 设 以 下 两 个 条 件 成 立 : 一 是 机 器 人 横向 速度 与 纵向 
速度 的 比值 很 小 ， 即 纵向 速度 远大 于 横向 速度 ; 二 是 航向 偏差 角 e. 信 小 。 再 
考虑 图 7. 2 中 车 辆 与 路 径 的 相对 运动 几何 关系 ， 有 : 
Pe =w -vp 2) 
y, = vQ +n, (7.3) 
在 航向 跟踪 控制 过 程 中 ， 可 以 令 道路 曲率 p =1/R 为 零 ， 即 不 计 实 际 路 径 
的 影响 。 由 式 (7.2), fi: 
e. = の (7.4) 
从 式 0.4) 可 以 看 出 ， 当 不 计 路 径 影响 时 ， 无 人 驾驶 车 辆 航向 变化 率 即 横 摆 角 
速度 。 
以 上 算式 联 立 即 可 得 出 以 横 摆 角速度 、 横 向 速度 、 航 向 偏差 前 和 前 轮 偏 角 


5rjs.cn 000000 


第 7 章 航向 跟踪 预 估 控 制 算法 





无 人 驾驶 车 辆 





图 7.1 无 人 各 驶 车 辆 与 道路 几何 关系 示意 


为 状态 变量 的 系统 状态 空间 表达 式 


7.3 航 问 预 估 算法 原理 


在 实际 的 航向 控制 过 程 中 ， 控 制 器 根据 期 望 航向 与 无 人 驾驶 车 辆 实际 航向 
得 到 航向 偏差 ， 再 计算 控制 量 ， 而 当 执行 机 构 执 行 这 一 控制 量 时 ， 要 经 过 一 个 
采样 周期 ， 这 时 无 人 驾驶 车 辆 的 实际 航向 已 经 改变 ， 即 控制 量 执行 时 已 有 一 个 
采样 周期 的 沾 后 ， 而 且 采 样 周期 一 定时 ， 无 人 驾驶 车 辆 纵向 速度 或 其 他 影响 因 
泰 不同， 航向 的 变化 量 也 不 一 样 ， 

模型 预测 控制 在 预测 过 程 中 设置 了 预测 时 域 。 普 通 的 MPC 控制 器 的 预测 
输出 也 是 利用 下 一 个 采样 周期 (预测 区 间 ) 的 预测 量 。 本 方法 也 利用 航向 变 
化 模型 在 下 一 周期 的 输出 ， 可 以 认为 是 一 种 没有 经 过 优化 校正 的 模型 预测 控制 
方法 

航向 预 估算 法 的 基本 思想 是 预测 无 人 和 驾驶 车 辆 航向 变化 趋势 ， 并 将 其 计 入 
探 制 侧 益 ， 这 样 航向 变化 趋势 就 可 以 影响 控制 器 的 输出 ， 即 无 人 驾驶 车 辆 的 前 
纶 偏 角 。， 本 书 介绍 的 是 一 种 在 简化 模型 基础 上 提出 的 预 估算 法 。 其 预 佑 模型 原 
理 如 图 7.2 所 示 ， 图 中 RR 表示 巨人 驾驶 车辆 绕 运 动 中 心 点 0 运动 的 半生 

由 式 (7.4) 可 知 ， 无 人 驾驶 车 辆 在 进行 航向 跟踪 控制 时 ， 只 要 求 出 无 人 
^5 5p Ae (7 Fo D ff EHE ,— ERE CPU e a A ff E fO. i fce] fd 2 m 变化 率 与 
采 伴 周期 的 乘积 即 无 人 名 驶 车 辆 在 一 个 采样 周期 内 的 航向 变化 晶 。 辱 设 控 - 
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图 7.2 二 自由 度 无 人 驾驶 车 辆 航向 预 估 模 型 


采样 周期 为 7 了， 纵向 速度 以 前 轮 速度 近似 ， 则 无 人 驾驶 车 辆 航向 在 一 个 控制 周 
期 的 変化 量 Ae(rad) 可 以 近似 地 计算 为 


Ap = vT/R (531 

前 轮 绕 运动 中 心 的 半径 尺 可 由 下 式 得 到 : 
R = (a + b)/sin(6,) (7.6) 

将 式 (7.6) 代入 式 (7.5)， 可 以 得 到 : 
Ago = vTsin(8,)/(a + b) (7.7) 


式 中 ， 纵 向 速度 v 和 前 轮 偏 角 61 是 在 当前 采样 周期 内 测 得 的 ， 在 下 一 个 控制 
周期 内 会 有 所 变化 ,但 由 其 变化 的 连续 性 计算 得 到 的 Aue 作为 下 一 个 控制 周 
期 内 无 人 驾驶 车 辆 的 航向 变化 佑 计量 是 可 行 的 。Aw 被 称 为 无 人 驾驶 车 辆 在 
下 一 个 控制 周期 内 的 航向 变化 预 估量 ， 以 下 简称 航向 预 估量 。 在 控制 算法 
中 ， 车 辆 当前 航向 与 航向 预 估量 之 和 作为 航向 反馈 量 ， 期 望 航向 与 航 问 反馈 
量 的 差 值 作为 控制 器 的 输入 偏差 。 航 向 预 估 控 制 框图 如 图 7. 3 所 示 。 


ーー 航向 ts 
N + | 控制 器 轮 偏 角 


无 人 驾驶 车 辆 





图 7.3 航向 跟踪 预 估 控 制 框 图 
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7.4 PID 控制 算法 


在 仿真 和 实验 中 ， 航 向 预 估 控 制 方法 和 与 之 对 比 的 常规 控制 方法 的 控制 器 
均 采 用 增 量 PID 算法 ， 且 两 者 的 比例 、 积 分 和 微分 系数 一 样 。 增 量 PID 算法 如 
X (7.8) 所 示 : 

Az = (た な) 一 (た ー1) 
= た |e( な ) -e(k-1)] +Ke(k) + Kile(k) -2e(k -1) *e(k -2)] 
(7.8) 
Hp Ko. Kj 和 天, 分 别 为 比例 、 积 分 和 微分 系数 ，u (k) 表示 第 た (た =0, 1, 
2，…) 个 采样 时 刻 的 控制 量 ，e (k) 表示 第 大 个 采样 时 刻 的 航向 输入 偏差。 


7.5 仿真 结果 


仿 直 实验 在 Matlab/Simulink 图 形 仿 直 环境 下 进行 ， 结 构 如 图 7.4 所 示 。 
与 实际 系统 一 致 ， 轮 式 无 人 驾驶 车 辆 前 轮 偏 角 最 大 设 为 35° ， 因 此 控制 器 最 
大 输出 绝对 位 置 控制 量 为 0.611rad。 控制 器 采样 周期 为 0.064 s， 每 一 控制 
周期 输出 增 量 最 大 为 0.0224rad。 仿 真 时 系统 参数 选取 如 表 7. 1 所 示 。 进 行 
航向 阶 牙 响 应 仿真 和 路 径 偏差 阶 跃 响应 ， 以 分 别 验证 航向 跟踪 性 能 和 路 径 
跟踪 性 能 
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表 7.1 航向 控制 仿真 实验 系统 参数 选取 


8 890 m m? 3 000 kg 48 000 N/rad | 42000 N/rad 1.56 m 


7.5.1 航向 阶 跃 响应 仿真 


将 初始 航向 偏差 设置 为 20。 (0.35rad), ， 进 行 航向 跟踪 阶 跃 响应 实验 。 仿 
真 时 取 2 种 纵向 速度 : 4 m/s 和 6 m/s. 2 种 纵向 速度 下 的 PID 捧 制 系 数 相同 . 
纵向 速度 为 4 m/s 有 、 无 航向 预 估 量 的 航向 阶 跃 响应 曲线 分 别 如 图 7.5 和 
图 7.6 所 示 。 纵 向 速度 为 6 m/s 有 、 无 航向 预 估量 航向 阶 跃 响 应 曲线 分 别 如 
图 7.7 和 图 7.8 所 示 。 











Heading/rad 








0 5 10 15 20 25 30 


Time/s | Time/s 
图 7.5 速度 为 4 m/s 时 有 预 估量 图 7.6 速度 为 4 m/s 时 无 预 估量 
航向 阶 跃 响应 fpc 16] [p EK] 
0.05 
0 


Heading/rad 
1 d 1 
EPI 
N t^ — 





Time/s Time fs 
图 7.7 速度 为 6 m/s 时 有 预 估量 图 7.8 速度 为 6 m/s INE JC üt fh hl 
航向 阶 跃 响应 fit 1] [fr IK ns] hv 


从 仿真 结果 可 以 看 出 ， 纵 向 速度 为 4 m/s 有 、 无 航向 预 估量 时 无 人 驾驶 车 
辆 航向 阶 跃 响应 曲线 基本 相同 ， 但 当 速度 变 为 6 m/s 时 ， 航 向 预 估 控 制 方法 的 
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阶 跃 响应 曲线 较为 理想 ， 而 用 常规 控制 方法 则 出 现 了 较 大 的 超 调 。 纵 向 速度 更 
号 时 ,两 种 方法 的 差别 更 明显 。 仿 真 结果 说 明 ， 有 航向 预 估 的 PID 控制 器 在 进 
行 航向 跟踪 时 适应 的 速度 范围 较 宽 ， 控 制 系统 的 鲁 棒 性 比 常规 控制 方法 强 。 


7.5.2 路 径 偏差 阶 跃 响应 仿真 


被 跟踪 路 径 为 一 平 直 道路 ， 无 人 驾驶 车 辆 的 初始 路 径 偏差 设置 为 -1 m 
( 负 值 表示 无 人 驾驶 车 辆 位 于 路 径 中 心 的 左边 ) ， 初 始 航 向 偏差 为 0， 仿 直 时 也 
取 两 种 纵向 速度 : 4 m/s 和 6 m/s。 两 种 纵向 速度 下 的 PID 控制 系数 相同 。 仿 
鼻 时 对 常规 方法 和 航向 预 估 方 法 的 结果 进行 了 对 比 。 速 度 为 4 m/s 和 6 m/s 时 
的 路 径 跟 踪 位 置 偏差 阶 跃 响应 曲线 分 别 如 图 7.9 和 图 7.10 所 示 ， 前 轮 偏 角 曲线 
分 别 如 图 7.11 和 图 7. 12 所 示 ， 航 向 偏差 曲线 分 别 如 图 7. 13 和 图 7. 14 所 示 。 
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图 7.13 速度 为 4 m/s 时 航向 偏差 曲线 图 7.14 速度 为 6 m/s 时 航向 偏差 曲线 


从 仿真 结果 可 以 看 出 ， 应 用 航向 预 估 控 制 方法 的 路 径 跟 踪 阶 跃 啊 应 曲线 较 
为 平滑 ， 而 且 当 纵向 速度 变化 时 ， 也 表现 出 了 较 好 的 适应 性 ; 从 对 控制 量 前 轮 
偏 角 的 响应 曲线 和 航向 偏差 曲线 来 看 ， 航 向 预 估 控 制 方法 的 稳定 性 及 对 纵向 速 
度 的 适应 性 明显 优 于 常规 方法 。 


7.6 实验 结果 


实验 平台 为 一 辆 具备 感知 与 车 辆 状态 测量 功能 的 无 人 驾驶 车 辆 。 车 辆 航向 
由 导航 系统 给 s 航向 精度 为 0. 1"。 车 辆 纵向 速度 由 速度 控制 系统 进行 控制 . 
实验 时 能 稳定 在 +0.2 m/s 以 内 。 与 仿真 一 样 ， 采 用 2 组 纵向 速度 进行 : 4 m/s 
和 6 m/s。 实 验 在 平坦 开阔 的 场地 进行 。 实 验 时 所 用 PID 参数 与 仿真 实验 相同 
另外 ， 为 了 验证 航向 预 估 算法 在 实际 路 径 跟踪 控制 中 的 优越 性 ， 进 行 了 基于 视 
觉 导 航 的 道路 跟踪 实验 。 实 验 过 程 中 纵向 速度 变化 范围 较 大 。 


7.6.1 实验 1 一 一 航向 阶 跃 实验 


开始 由 无 人 驾驶 车 辆 跟踪 一 给 定 航向 ， 达 到 期 望 速度 且 航 向 稳定 后 ， 给 出 
20° (0.35rad) 的 阶 跃 偏差 。 纵 向 速度 为 4 m/s 有 、 无 航向 预 估量 的 航向 阶 唉 
响应 曲线 分 别 如 图 7. 15 和 图 7. 16 所 示 。 纵 向 速度 为 6 ms 有 、 无 航向 预 估量 
的 阶 跃 响应 曲线 分 别 如 图 7.17 和 图 7. 18 所 示 。 

实验 结果 与 仿真 结果 基本 一 致 。 纵 向 速度 为 4 m/s 时 ， 两 种 方法 的 阶 跃 响 

应 曲线 基本 相同 ， 但 预 估 控 制 响应 曲线 较为 平滑 。 纵 向 速度 为 6 m/s 时 ， 预 估 
控制 方法 控制 效果 与 速度 为 4 m/s 时 差别 不 大 ， 但 常规 控制 方法 阶 跃 响应 曲线 
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ーー desired heading 


ーー desired heading — measured heading 


— measured heading 





0 100 200 300 400 500 600 0 100 200 300 400 500 600 700 
时 间 / 0X0.0648) 时 间 / (X0.064s) 
图 7.15 速度 为 4 ms 时 有 预 图 7.16 速度 为 4 m/s 时 无 预 
估量 航向 阶 跃 响应 估量 航 向 阶 跃 响应 


航向 / Co 


ーー desired heading 
— measured heading 


0 S0 100 150 200 250 300 350 400 0 S0 100 
時 同 /(X0.064s) 


ーー desired heading 
— measured heading 


150 200 250 300 350 











时 间 / (X0.064s) 
图 7.17 速度 为 6 m/s 时 有 预 图 7.18 速度 为 6 m/s 时 无 预 
估量 航向 阶 跃 响应 估量 航向 阶 跃 响应 


则 出 现 了 明显 的 振荡 。 同 时 ， 从 图 7.17 和 图 7. 18 的 直线 跟踪 阶段 曲线 即 阶 
跃 响应 之 前 的 曲线 ) 可 以 看 出 ， 纵 向 速度 为 4 m/s 时 ， 航 向 预 估 控 制 方法 的 跟 
踪 曲 线 较 为 平滑 ， 速 度 为 6 m/s 时 也 有 同样 的 效果 


7.6.2 实验 2 一 一 航向 连续 跟踪 实验 


实验 过 程 中 ， 每 5 个 控制 周期 (5 x0.064 s) 期 望 航向 减少 1? ， 减 少 20* 
后 期 望 航向 保持 不 变 。 设 计 本 实验 的 日 的 是 模拟 弯 道 跟踪 。 实 验 时 纵向 速度 保 
持 在 6 m/s 有 、 无 预 估量 的 航向 响应 曲线 分 别 如 图 7.19 和 图 7.20 所 示 。 

从 图 中 可 以 看 出 ， 航 向 预 估 控 制 方法 的 响应 曲线 较为 平滑 ， 且 无 超 调 ， 能 
很 好 地 完成 连续 跟踪 任务 ; 而 常规 控制 方法 则 出 现 了 大 幅 振荡 ， 且 曲线 不 
平滑 
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25 
20 
,15 
E: 
z 
E 10 、 
ーー desired heading N 5f -- desired heading ' 
— measured heading 0 — measured heading 
0 50 100 150 200 250 300 350 0 50 100 150 200 250 300 350 
时 间 / 0X0.064 s) 时 间 / (x0.064 s) 
图 7.19 纵向 速度 为 6 m/s 时 有 预 估量 图 7.20 纵向 速度 为 6 m/s 时 无 预 估量 
航向 连续 跟踪 响应 曲线 航向 连续 跟踪 响应 曲线 


从 上 面 的 实验 可 以 看 出 实验 结果 与 仿真 结果 基本 一 致 。 这 说 明 对 无 人 驾驶 
车 辆 转向 控制 的 仿真 结果 是 可 信和 的。 


7.6.3 ”实验 3 一 一 航向 预 估算 法 在 路 径 跟 踪 控制 中 的 应 用 


本 实验 目的 是 为 了 验证 航向 预 估算 法 在 路 径 跟 踪 控 制 中 的 优越 性 。 被 跟踪 
路 径 由 视觉 导航 系统 给 出 ， 而 局 部 规划 层 则 根据 视觉 图 像 处 理 结果 得 到 可 行驶 
的 路 径 点 ， 即 期 望 路 径 。 差 分 GPS 系统 提供 的 全 局 坐标 精度 可 达到 0. 1 m、 通 
过 下 面 的 推算 可 以 得 到 期 望 路 径 坐 标点 

实验 采用 基于 航向 的 路 径 跟 踪 方 法 ， 预 视 距 离 d 的 选取 与 速度 旦 线性 关系 : 

d -d,*k,v (7.9) 

其 中 ，d 是 纵向 速度 为 0 时 的 预 视 距 离 ， 被 称 为 基本 预 视 距 离 ; ,是 纵向 速度 
关联 系数 ， 实 际 选取 时 不 宜 过 大 。 

控制 器 采用 第 7. 4 节 所 述 的 增 量 PID 控制 算法 。 为 了 说 明 航 向 预 估 算法 对 
纵向 速度 的 适应 性 ， 在 纵向 速度 为 4 m/s, 8 m/s 和 12 m/s 时 分 别 整定 了 3 组 
PID 参数 。 实 验 时 无 人 驾驶 车 辆 纵向 速度 在 0 ~ 14 m/s 变化 。 采 用 如 下 规则 : 
当 纵 向 速度 为 0<v <6 m/s 时 ，PID 控制 参数 采用 4 m/s 时 整定 的 参数 ， 当 纵 
向 速度 为 6 < 10 m/s 时 ，PID 控制 参数 采用 8 m/s 时 整定 的 参数 ; 当 纵 向 
速度 为 10 «v, <14 m/s 时 ，PID 控制 参数 采用 12 m/s 时 整定 的 参数 。 

无 人 驾驶 车 辆 全 自主 行驶 的 道路 跟踪 曲线 如 图 7.21 (a) 所 示 ， 显 示 的 期 
望 路 径 是 根据 预 视 距离 选择 的 目标 路 径 点 列 。 网 中 的 点 (0, 0) 为 差分 GPS 
基站 接收 机 所 处 位 置 。 图 7.21 (b) 是 图 7.21 (a). 中 方 框 部 分 的 放大 ， 
图 7.21 (e) 是 期 望 航向 及 无 人 驾驶 车 辆 实际 跟踪 航向 曲线 ， 而 图 7.21 (d) 
则 是 无 人 驾驶 车 辆 全 程 的 速度 曲线 。 从 实验 效果 进一步 证 明了 航向 预 估 算法 对 
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纵 癌 速度 较 宽 的 适应 范围 。 若 采用 常规 PID 控制 ， 则 纵向 速度 每 变化 1 m/s 就 
必须 重新 整定 PID 参数 ， 否 则 难以 达到 满意 的 路 径 跟 踪 效 果 。 
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(d) 


图 7.21 基于 视觉 导航 的 无 人 驾驶 车 辆 道路 跟踪 实验 ( 续 ) 
(c) 航向 跟踪 曲线 ; (d) 纵向 速度 曲线 


本 章 提出 一 种 无 人 驾驶 车 辆 航向 跟踪 预 估 控 制 算 法 ,根据 无 人 驾驶 车 辆 
前 轮 偏 角 和 纵向 速度 来 计算 航向 的 变化 量 作 为 航向 预 估量 ， 并 将 航向 预 估 量 
与 无 人 驾驶 车 辆 实际 航向 之 和 作为 控制 的 反馈 航向 。 航 向 预 估量 对 无 人 驾驶 
车 辆 航向 变化 趋势 做 了 预测 。 由 于 在 航向 预 估量 中 考虑 了 无 人 驾驶 车 辆 纵向 
速度 和 实时 前 轮 偶 角 的 影响 ， 因 而 控制 器 对 无 人 驾驶 车 辆 纵向 速度 变化 的 适 
应 范围 较 宽 。 算 法 简单 实用 ， 几 乎 没有 添加 任何 计算 量 ， 实 时 性 好 。 在 仿真 
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实验 和 实 车 实验 中 ， 把 用 航向 预 估算 法 和 PID 控制 算法 相 结 合 ， 进 行 无 人 驾 
驶 车 辆 的 航向 跟踪 和 路 径 跟踪 实验 。 结 果 表 明 ， 与 常规 方法 相 比 ， 航 向 预 佑 
算法 有 效 地 改善 了 控制 性 能 ， 适 应 的 纵向 速度 范围 较 常规 控制 方法 要 宽 ， 对 
系统 参数 变化 的 敏感 性 较 低 ， 减 少 了 控制 器 对 无 人 驾驶 车 辆 模型 的 依赖 ， 提 
高 了 控制 器 的 鲁 棒 性 。 
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| CarSim 8. 02 应 用 高 版 本 Matlab 


首先 确认 您 的 Matlab 是 64bit 还 是 32bit。 以 下 以 Matlab R2012b ( 64bit ) 
为 例 : 

CD 选择 Models: Simulink, ， 如 图 A. 1 所 示 。 

D 选择 红色 设置 CarSim 和 Simulink 链接 工程 文件 ， 如 图 A. 1 所 示 
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Mechanical Simulation 


图 A.1 
出現 Simulink 设置 对 话 框 ， 如 图 A. 2 所 示 ; 选择 For 64 - bit Windows OS: 
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Use 64 - bit Matlab， 同 时 保证 Simulink Model 的 后 级 为 .mdl， 并 且 路 径 均 为 

英文 ビー 
返回 Matlab 对 话 框 ， 如 图 A. 3 所 示 ; 选择 “SetPath” 添 加 CarSim 路 径 下 

TI ft dE Matlab 路 径 


Set Path 





图 A.3 


选择 “Add with Subfolders”， 找 到 CarSim fljoK fit de SFI, RIME Matlab 
路 径 中 ， 例 如 (C: V ProgramFiles ( x86) V CarSim802 | Prog V Programs V Solv- 
ers) 保存 设置 (Save) 如 图 A. 4 所 示 
重启 电脑 ， 重 新 打开 Matlab 的 Simulink Library Browser ( 如 图 A.S 所 示 ) ， 
CarSim 模块 就 出 现在 Library 里 面 了 了 
HEA CarSim 模块 进入 Simulink 工程 中 ， 保 存 工 程 选择 文件 后 级 为 .mdl 
( CarSim 8. 02 不 支持 后 级 为 .slx 的 文件 和 中 文 路 径 ) 
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C: \ProgramFiles (x86) \CarSim802_Prog \ Programs \ Solvers \ Default 
C: \ProgramFiles (x86) \CarSim802_Prog\Programs\Solvers 

C: \ProgramFiles (x86) \CarSim802_Prog \Programs \ Solvers V Defaultó4 
C: \ProgramFiles (x86) \CarSim802_Prog \ Programs \ Solvers \ReadMe 


og Program 
giProgramsAsolvecsVDelaultó4 
Prog Prodramsisalvers ReadMe 


& CAProgram Files;MATLABAR2013bNtoolboxihdlcoderymatlabhdlcoder matiabhdlcoder 

— —— L CAProgram Files;MATLABYR2013bMtoolboxihdlcoder matlabhdlcoder 
Meis Tap 」 CAProgram Files\MATLAB\R2013b\toolbox\matlab\testframework 

L CVProgram Files\MATLAB\R2013b\toolbox\matlabx\matiabx! 

」 C:\Program FilesNMATLABNR2013bMoolbowmatlabxfvmatlabxldemos 

hp C:\Program Files\MATLAB\R2013b\toolbox\matiab\demos 

^ CAProgram Files\MATLAB\R2013b\toolbox\matlab\graph2d 

上 C:\Program FlesNMATLABNR2013b\toolboxvnatlab\graph3d 

i CAProgram Files; MATLABYR2013bMtoolboxVmatlab graphics 
L CAProgram Files MATLABYR2013bWtoolboxymatlab plottocls 
i 
4 








CAProgram Files\MATLAB\R2013b\toolbox\matlab\scribe 
C:\Program Files\MATLAB\R2013b\toolbox\matlab\specgraph 
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e: 车 体 的 横 摆 角 (航向 角 ) 
8: 前 轮 偏转 角 
8,: 后 轮 偏转 角 


Aó,: 每 个 控制 周期 的 前 轮 偏 角 增 量 


a; 轮胎 侧 偏 角 

c 前 轮 侧 偏 角 

: 后 轮 侧 偏 角 

B: 质心 侧 偏 角 

vo: 车 辆 后 轴 中 心 速度 
us 车 辆 前 轴 中 心 速度 
v.s 轮胎 侧 向 速度 
v.s 轮胎 纵向 速度 

ELT 

L: 车 长 

W. 车 宽 

a, b: 质心 到 前 、 后 轴 的 距离 
ん: 车 辆 绕 > 轴 的 转动 惯量 
m; 车 辆 整备 质量 

R: 车辆 转向 半径 

r 车 轮 半 径 

Fs; 轮胎 纵向 力 
F.: 轮胎 侧 向 力 
( 
( 
[1 


2 


只 


^: 轮胎 纵向 刚度 
し AE Me I f E JE 
^ Cus M. Jie e n fI] HE 
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, Cui 前 、 后 轮胎 纵向 刚度 
: 前 、 后 轮胎 受到 的 纵向 力 
Fa: 前 、 后 轮胎 受到 的 侧 向 力 
F,.: 前 、 后 轮胎 受到 的 x 方向 的 力 
F,.: 前 、 后 轮胎 受到 的 y 方 向 的 力 
: 轮胎 所 受到 的 垂 向 载荷 
F.: 前 、 后 轮胎 受到 的 垂 向 载荷 
: 轮胎 翻转 力矩 
: 轮胎 阻力 矩 
.: 轮胎 回 正 力矩 
B; 魔术 模型 中 曲线 的 刚度 因子 
C: 魔术 模型 中 曲线 的 形状 因子 
D: 魔术 模型 中 曲线 匮 因子 
E; 魔术 模型 中 曲线 的 曲率 因子 
S,: 曲线 的 水 平方 向 漂移 
5,: 曲线 的 垂直 方向 漂移 
w: "ESI fü HE 
w: 车 轮 旋 转角 速度 
u: 地 面 附着 系数 
s: 纵向 滑 移 率 
を: 状态 变量 
u; 控制 变量 
n: 系统 输出 
£u: 运动 学 模型 中 的 状态 量 
Uyni 运动 学 模型 中 的 控制 量 
Nein: 运动 学 模型 中 的 输出 量 
£u: 动力 学 模型 中 的 状态 量 
u,,: 动力 学 模型 中 的 控制 量 
Mayn : 动力 学 模型 中 的 输出 量 
£s: 点 质量 模型 中 的 状态 量 
mi 点 质量 模型 中 的 控制 量 
T: 采样 周期 
h: JER - 库 塔 迭 代 算法 中 的 步 长 
N: 时 域 (包含 预测 时 域 和 控制 时 域 ) 


= 


Ri) 
EI 
E 
«=| 


一 ps = 
的 n B 


ROOGEOE uw Nn 
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V,: 预测 时 域 
Vi 控制 时 域 
u^ (1): t 时刻 最 优 控 制 量 
U' (0: 4 时 刻 最 优 控制 序列 
EE 
Eas : 系统 输出 序列 约束 
kay 0): 在 上 时 刻 对 :+ 上 时 刻 的 预测 状态 量 
を 参考 系统 的 状态 变量 
系统 的 控制 变量 
poe 
Us : 控制 量 约束 
es TT 约束 
ay Hays ss: 动力 学 模型 中 控制 量 约束 
Au. Au: 控制 增 量 极 约束 
Ausus Ai 运动 学 模型 中 控制 增 量 约束 
ee RSEN 
: 状态 变量 维 数 
: 控制 变量 维 数 
X: pre 量 约束 
D: 控制 变量 约束 
n.a: 系统 参考 输出 
Marai 局 部 参考 输出 
y: 便 约 束 输出 
Yi oiu 
iei o rbi acies 
dy pant 软 约 束 输出 极限 值 
e: 陡 测 时 域内 的 跟 踩 误差 
A,, B,: 41 时刻 的 转移 和 矩 阵 
Ari B,,: 4 时 刻 对 时 刻 预测 的 转移 矩阵 
Au CK), B,,Ck) :线性 运动 学 模型 转移 矩阵 的 离散 形式 
Aq), BuU :时刻 线 性 动力 学 模型 的 转移 矩阵 〈 连 续 形 式 ) 
4 (た だ) (た): 线性 动力 学 模型 转移 矩阵 的 离散 形式 
Ya, Qui 双 移 线 参考 轨迹 
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oi 车 辆 纵向 加 速度 

,: 车 辆 侧 向 加 速度 

: 松弛 因子 

: 松弛 因子 的 权重 系数 

as: 避 障 函数 的 权重 系数 

“: 较 小 的 正 数 

a,, b,: 5 次 曲线 拟 合 中 待 求 系数 和 矩阵 


四 了 mA a 
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